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19.  Appendix  N:  EXOSIM  2.0  (End-to-end) 

FILE:  uuv22 . 1 9g/debug/make f i le 


FORFLAGS  =  code  large  oDtimize(3)  storage (integer-2) 
DUTILITY  =  "V  DUTILITY /UTILITY. LIB 
SUTILITY  =  '/SUTILITY/UTILITY.LIB 


OUOGRAM  =  \ 

SSBLK00.BL  \ 
SSBLKQ1.BL  \ 
SSBLK02 . BL  \ 
SSBLKC3.BL  \ 
SSBLK04.BL  \ 
SSBLK05.BL  \ 
SSBLK06.BL  \ 
SSBLK07.BL  \ 
SSBLK08 . BL  \ 
SSBLK09 . BL  \ 
SSBLK10.BL  \ 
SSBLK11.BL  \ 
SSBLK12 . BL  \ 
SSBLK1 3 . BL  \ 
SSBLK1 4 . BL  \ 
SSBLK1 5 . BL  \ 
SSBLK1 6 . BL  \ 
SSBLK1 7 . BL  \ 
SSBLK1 8 . BL  \ 
CROSSBAR. BL  \ 
SEQUENCER. BL 


default:  S (PROGRAM) 


SSBLK00 . BL : UUBLKOO.OBJ  S (DUTILITY) 

submit  : PFP : csd/forbldlnew (  SSBLKOO,  ' UUBLKOO . OBJ,  $ (DUTILI TY )  ',  notype  ) 


SSBLK01 . BL: UUBLKOl.OBJ  S (SUTILITY) 

submit  : PFP : csd/forbldlnew (  SSBLK01,  • UUBLK01 .OBJ, S (SUTILITY) ' ,  notype  ) 


SSBLK02 . BL : UUBLK02 . OBJ  S  (SUTILITY) 

submit  : PFP : csd/forbldlnew (  SSBLK02 ,  •  UUBLK02 . OBJ, $ (SUTILI TY)  ' ,  notype  ) 


SSBLK03 . BL: UUBLK03 . OBJ  S (SUTILITY) 

submit  : PFP : csd/ forbldlnew (  SSBLK03,  ' UUBLK03 .OBJ, $ (SUTILITY) ' ,  notype  ) 


SSBLKO  4  .  BL :  UUBLK04 .  OBJ  $  (DUTILITY) 

submit  :  PFP :  csd/  forbldlnew  (  SSBLK0<!,  '  UUBLKO 3  .  OBJ,  S  (DUTILITY)  ' ,  notype  ) 


SSBLK05 . BL: UUBLK05 . OBJ  3 (SUTILITY) 

submit  : PFP : csd/forbldlnew (  SSBLK05,  • UUBLK05 . OBJ, S (SUTILITY )' ,  notype  ) 


SSBLKO 6 . BL :  UUBLKO 6 . OBJ  $ (SUTILITY) 

submit  : PFP : csd/ forbldlnew (  SSBLK06,  ' UUBLK06 . OBJ, $ (SUTILITY) ' ,  notype  ) 


SSBLK07 . BL : UUBLK07 . OBJ  $ (SUTILITY) 

submit  : PFP : csd/forbldlnew (  SSBLK07,  • UUBLK07 .OBJ, S (SUTILITY) ' ,  notype  ) 


SSBLKO 8 . BL: UUBLKO 8 .OBJ  S (SUTILITY) 

submit  :PFP:csd/forbldlnew(  SSBLK08 ,  • UUBLK08 . OBJ,  S (SUTILITY)  ' ,  notype  ) 


SSBLK09 . BL:  UUBLKO 9 . OBJ  S (SUTILITY! 

submit  : PFP : csd/ f o rb Idl new (  SSBLK09,  'UUBLK09 . OBJ, S (SUTILITY) ' ,  notype  ) 


SSBLK 1 0 . BL : UUBLKIO.OBJ  $ (SUTILITY) 

submit  :PFP: csd /forbldlnew!  SSBLK10,  ' UUBLK1 0 . OBJ, S ( SUTI LITY) ' ,  notype  ) 
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SSSLKl 1 . BL : UUBLK11 .OBJ  5 (DUTILITY) 

submit  :PFP :csd /forbldlnew (  SSBLKli,  •  UUBLK1 1 . OBJ,  S  (DUTILITY)  • ,  n.^ype  ) 


SSBLK12 . BL : UUBLK12 . OBJ  5 (DUTILITY) 

submit  : PFP : csd/ forbldlnew (  SSBLK12,  • UUBLK12 . OBJ, $ (DUTILITY) ' ,  notype  ) 


SSBLK13.BL:  UUBLK1 3 . OBJ  S  (SUTILITY) 

submit  : PFP : csd/f orbldlnew (  SSBLK13,  ■ UUBLK1 3 . OBJ,  $ (SUTILITY)  • ,  notype  ) 


SSBLK1 4 . BL : UUBLK14.0BJ  $ (SUTILITY) 

submit  : PFP : csd/ forbldlnew (  SSBLK14,  ' UUBLK1 4 . OBJ, $( SUTILITY) ' ,  notype  ) 


SSBLK1 5 . BL:  UUBLKIS.OBJ  $  (DUTILITY) 

submit  :PFP : csd/ forbldlnew (  SSBLK15,  ' UUBLK1 S . OBJ, $ (DUTILITY ) 1 ,  notype  ) 


SSBLKl 6. BL: UUBLK1 6 . OBJ  $ (SUTILITi) 

submit  : PFP : csd/f orbldlnew (  SSBLKl 6,  • UUBLK1 6 . OBJ,  $ (SUTILITY)  ■ ,  notype  ) 


SSBLKl 7 . BL: UUBLK1 7 . OBJ  S (SUTILITY) 

submit  : PFP : csd/f orbldlnew (  SSBLKl 7,  ' UUBLK1 7 . OBJ, $ (SUTILITY) 1 ,  notyne  ) 


SSBLK18.BL: UUBLKIS.OBJ  S (SUTILITY) 

submit  : PFP : csd/ forbldlnew (  SSBLKl 8 ,  ' UUBLK1 8 . OBJ, $ (SUTILITY )• ,  notype  ) 


CROSSBAR. BL  SEQUENCER . BL -  NETWORK . TXT 
submit  : PFP : csd/xbc  (  network.txt  ) 


. f or . ob j : 

ftn286.new  $<  S(forfiags) 


clean : 

delete  *.obj,*.)st,*.mpl,*. mp2, * . bl 


run : 

reset 

download  process.txt 

start  process.txt 

ioserve  process.txt  3600  -debug 


FILE:  uuv22.1 9g/ debug /net work . txt 


LOOP 

CYCLE  [  1  ] 

p05,  pi 3  :=  p00.4;  [  REAL* 8  XD  1000  ] 

CYCLE  f  2  1 

p08  :=  p00.2;  (  REAL*  4  XD_  1001  | 

CYCLE  [  3  ] 

p05,  pi 3  :=  p00.4;  [  REAL*8  YD  1002  ] 

CYCLE  t  4  ! 

p08  :=  p00.2;  (  REAL*  4  YD_  1003  ] 

CYCLE  [  5  J 

pO  5 ,  pi  3  :»  p00.4;  (  REAL*  8  ZD  1004  ] 

CYCLE  [  6  1 

p08  poo. 2;  [  REAL*  4  ZD_  1005  ) 

CYCLE  [  7  ] 

pO  5 ,  pi  3  :  =  p00.4;  (  REAL*  8  X  1006  ] 

CYCLE  [  8  1 

p02,  p08,  pio  : -  p00.2;  [  REAL* 4  X_  1007  ) 
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CYCLE 
pO  5 , 

[  9  1 

pl  3  •-  [  REAL*  8  Y  1008 

l 

CYCLE 

p02. 

[  10  ’ 
pG8,  plO 

:=  p00.2 

t  REAL*  4  Y_ 

1009  ] 

CYCLE 
pO  5 , 

E  n  1 

pl 3  :=  p00.4;  [  REAL* 8  Z  1010 

1 

CYCLE 
p02 , 

[  12  } 
pQ8,  plO 

:=  p00.2 

[  REAL* 4  Z_ 

1011  ! 

CYCLE 
p04 , 

!  13  1 

pl 3  :=  pl0.2;  [  REAL* 4  P  1012 

1 

CYCLE 

p04. 

[  14  I 
p05,  pl3 

:=  pl0.2 

[  REAL* 4  0 

1013  ] 

CYCLE 
p04 , 

(  15  1 
p05,  pl3 

:=  pl0.2 

i  REAL* 4  R 

1014  ) 

CYCLE 

pOO 

[  16  1 
:  -  pl 0 . 7 

[  REAL* 4 

QUAT(l)  1015 

i 

CYCLE 

pOO 

E  11  1 

:=  pl0.2; 

[  REAL* 4 

QUAT ( 2 )  1015 

I 

CYCLE 

pOO 

t  18  ] 

:  =  pl  0 . 2 

[  REAL* 4 

QUAT(3>  1015 

] 

CYCLE 

pOO 

(  19  | 

:=  pl0.2; 

[  REAL* 4 

QUAT (4)  1015 

1 

CYCLE 
pi  9 

(  20  ] 

:=  p00.4; 

[  REAL*8 

MASS  a  01 6  ) 

CYCLE 

p06. 

[  21  1 
plO,  p2 1 

:=  pOO . 2 

(  REAL*  4  MASS_  1017  ] 

CYCLE 
p04  , 

[  22  ] 
p05,  p08. 

plO,  pl3 

:=  p00.2;  ( 

REAL* 4 

CIMU) 

1018 

CYCLE 
p04 , 

(  23  ] 
p05,  p08. 

plO,  pl 3 

:  =  p00.2;  [ 

REAL* 4 

CIM (2) 

1018 

CYCLE 

p04. 

[  24  1 
p05,  p08. 

plO,  pl  3 

:=  p00.2;  [ 

REAL* 4 

CIM (3) 

1018 

CYCLE 

p04. 

[  25  1 
p05,  p08. 

plO,  pl 3 

:=  pOO  .  2;  ( 

REAL* 4 

CIM (4) 

1018 

CYCLE 

p04. 

[  26  1 
p05,  p08. 

plO,  pl 3 

:=  p00.2;  [ 

REAL*  4 

CIM ( 5 ) 

1018 

CYCLE 

p04. 

[  27  ] 
p05,  p08. 

pl 0 ,  pl 3 

:=  p00.2;  [ 

REAL*4 

CIM  { 6) 

1018 

CYCLE 
p04 , 

[  28  1 
p05,  p08. 

plO,  pl 3 

:=  p00.2;  [ 

REAL*  4 

CIM ( 7 ) 

1018 

CYCLE 
p04 , 

(  29  1 
p05,  p08. 

plO,  pl 3 

:=  p00.2;  l 

REAL* 4 

CIM  (8) 

1018 

CYCLE 
p04  , 

[  3C  1 

p05,  p08. 

plO,  pl3 

:=  p00.2;  [ 

REAL* 4 

CIM  (9) 

1018 

CYCLE 
pl  3 

[  3i  j 

:=  pOO . 4 , 

[  REAL* 8 

UD  1019  1 

CYCLE 
pl  3 

t  32  ] 

:=  p00.4. 

i  REAL'S 

VD  1022  1 

CYCLE 
Pl  3 

(  33  ;• 

:=  p00.4. 

(  REAL*8 

WD  1024  I 

CYCLE 
pl  3 

[  34  ] 

:  =  pl0.2; 

[  REAL*  4 

PD  1020  : 

CYCLE 
pl  3 

[  35  ] 

pl0.2. 

!  REAL*  4 

QD  1021  ] 
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CYCLE 

pl  3 

[  36 

1 

plO . 2; 

[  REAL* 4 

RD  1023  j 

CYCLE 

pl3 

[  31 

1 

pOO. 4; 

[  REAL* 8 

GRU)  1025  ) 

CYCLE 

pl3 

[  38 

! 

pOO. 4; 

(  REAL*8 

GR<2)  1035  t 

CYCLE 
pi  3 

[  39 

1 

pOO . 4; 

L  REAL* 8 

GR ( 3 )  1025  ] 

CYCLE 

pl  3 

[  40 

) 

pOO.  4; 

[  REAL*  8 

XYZE(l)  2000  1 

CYCLE 
pl  3 

t  41 

1 

pOO .4; 

!  REAL* 8 

XYZE (2)  2000  ] 

CYCLE 
pl  3 

[  42 

) 

p00.4; 

[  REAL* 8 

XYZE ( 3 )  2000  1 

CYCLE 

p08 

t  « 

1 

pOO . 2; 

[  REAL* 4 

XYZE_ { 1 )  2001 

1 

CYCLE 

p08 

[  44 

1 

p00.2; 

E  REAL *4 

XYZE_<2)  2001 

) 

CYCLE 

p08 

(  45 

1 

p00.2; 

[  REAL* 4 

XYZE_ (3)  2001 

1 

CYCLE 

P1 3 

[  46 

I 

pOO.  4; 

(  REAL* 8 

XYZED(l)  3000 

j 

CYCLE 
pl  3 

[  47 

1 

pOO.  4; 

[  REAL*  8 

XYZEO ( 2 )  3000 

1 

CYCLE 
pl  3 

t  48 

! 

pOO .  4 ; 

[  REAL* 8 

XYZED ( 3 )  3000 

) 

CYCLE 
pO  9 

[  49 

1 

pOO . 2; 

[  REAL* 4 

Ej.oP  4000  ] 

CYCLE 

p06. 

(  50 
p09, 

I 

p26, 

pl 3 ,  pl 5 , 

p25,  p2  3 

pOO . 2; 

(  REAL *4 

CG(l) 

4001 

CYCLE 

p06. 

[  51 
p09, 

1 

,  p26, 

pl3,  pl 5 , 

p25,  p2  3  :  = 

p00.2; 

[  REAL* 4 

CG  ( 2 ) 

4001 

CYCLE 

p06. 

[  52 
p09, 

] 

•  p2  6 , 

pl 3 ,  pl5, 

p25,  p23  := 

p00.2; 

[  REAL* 4 

CG  (3) 

4001 

CYCLE 

plO, 

[  53 
pl  9 

1 

:=  p00.2;  [  REAL* 4  IXX  4002 

I 

CYCLE 

p06, 

[  54 
plO, 

) 

,  pl  9 

:=  p00.2, 

:  [  REAL* 4  IYY 

4003  i 

CYCLE 

p06. 

t  55 
plO, 

1 

.  pl9 

:=  p00.2, 

:  (  REAL* 4  IZZ 

4004  1 

CYCLE 

pOO, 

[  56 
pl  0 

1 

:=  p09.2;  £  REAL* 4  FXT  4006 

! 

CYCLE 

pOO 

[  57 

1 

p09. 2; 

[  REAL* 4 

FYT  4008  ] 

CYCLE 

pOO 

i  58 

1 

pO  9 . 2 ; 

[  REAL*  4 

FZT  4010  ] 

CYCLE 
pl  0 

[  59 

1 

p09. 2; 

[  REAL*  4 

MXT  4012  ] 

CYCLE 

plO 

[  60 

1 

pO  9 . 2  ; 

[  REAL* 4 

MYT  4014  ) 

CYCLE 

plO 

[  61 

1 

pO  9 . 2 ; 

(  REAL* 4 

MZT  4016  ] 

CYCLE 

pOO 

[  62 

pO  9 . 2 ; 

[  REAL* 4 

MDOTT  4018  ( 
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CYCLE 

pOO, 

1  63 
pi  0 

1 

p2  3 

.2;  [  RErtL*  4  FRCX  4005 

CYCLE 

pOC 

[  64 

P2  3 . 2 ;  [ 

REAL*  4 

FRCY  4007  ) 

CYCLE 

pOO 

[  65 

1 

P23.2;  [ 

REAL*  4 

FRCZ  4009  ) 

CYCLE 
pi  0 

[  66 

i 

P23.2;  [ 

REAL* 4 

MRCX  4011  ] 

CYCLE 

plO 

[  61 

] 

P  2  3 . 2 ;  ; 

REAL* 4 

MRCY  4013  1 

CYCLE 

plO 

:  68 

P23.2;  [ 

REAL* 4 

MRCZ  4015  ] 

CYCLE 

pCO 

[  69 

1 

P23.2;  [ 

REAL* 4 

MDOTF  4017  1 

CYCLE 
pO  9 

[  10 

l 

p23.2;  [ 

REAL* 4 

FCFFl(l)  5000 

CYCLE 

p09 

L  71 

p2  3 . 2 ;  ( 

REAL* 4 

F0FF1 { 2 )  5000 

CYCLE 
pO  9 

[  72 

1 

p2  3 . 2 ;  [ 

REAL* 4 

FCFF1 ( 3 )  5000 

CYCLE 
pO  9 

l  73 

p2  3 . 2 ;  [ 

REAL*  4 

F0FF1 (4)  5000 

CYCLE 
pO  9 

!  74 

1 

p2  3 . 2 ;  [ 

REAL*  4 

F0FF2 { 1 )  5001 

CYCLE 
pO  9 

[  75 

p2  3 . 2 ;  [ 

REAL* 4 

F0FF2 ( 2 )  5001 

CYCLE 

p09 

i  76 

1 

p2  3 . 2  ;  [ 

REAL* 4 

F0FF2  ( 3 )  5001 

CYCLE 

p09 

t  71 

! 

P23.2;  [ 

REAL*  4 

FOFF2  (  4 )  50C1 

CYCLE 

p05 

[  78 

1 

pO  8 . 2 ;  [ 

REAL*  4 

CER(l)  6000  ] 

CYCLE 
pO  5 

!  19 

1 

pC8 . 2 ;  i 

REAL* 4 

-Ek(2)  6300  | 

CYCLE 
pO  5 

[  60 

i 

pO  8 . 2  ;  [ 

REAL* 4 

CER  1 3 )  6000  1 

CYCLE 
pO  5 

[  81 

1 

p08.2;  [ 

REAL* 4 

CER ( 4 )  6000  ] 

CYCLE 
pO  5 

;  8? 

1 

p08 . 2;  ( 

REAL* 4 

CER ( 5 )  6000  ) 

CYCLE 

pOS 

t  83 

p08 . 2 ;  { 

REAL*  4 

CER ( 6 )  6000  ) 

CYCLE 
pO  5 

;  84 

p08 . 2 ;  [ 

REAL*  4 

CER (7)  6000  1 

CYCLE 
pO  5 

[  85 

i 

p08 . 2;  ' 

REAL*  4 

CER ( 8 )  6000  1 

CYCLE 
pO  5 

[  86 

1 

p08.2;  ( 

REAL*  4 

CER  f  9 }  6000  1 

CYCLE 

P02, 

[  81 
Pi  2 

] 

pO  8 

.2;  [  REAL* 4  ALT  10CC 

CYCLE 
p  J  9 
pi  4 

£  88 

1 

p  1 2 . 2 ;  : 

p05.4;  [ 

REAL*  4 
REAL*  8 

PRESS  8001  ; 

GRTC.  ,  1)  8002 

CYCLE 

I  89 

; 
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p  14 
p  ]  5 

:  =  p  0  S .  4 ;  ; 

:=  pl  7 . 2 ;  [ 

REAL* 8 
REAL*  A 

GRT  (1,2) 
RHO  8C03 

8002  ; 

1 

CYCLE 
pi  4 

P1  5 

90  ! 

:=  pOS.4;  [ 

:=  p  1 2 . 2 ;  [ 

REAL* 8 
REAL* A 

GRT  (1,3) 
VSND  8004 

8002  ; 

CYCLE 
d2  3 
pi  4 

91  1 

:=  pl 5 . 2;  [ 

:=  p05 . 4 ;  [ 

REAL* A 
REAT  *8 

MACH  8006 
VTIC (1, 1) 

8oic  : 

CYCLE 

p23 

pl-t 

92  ! 

:=  pl5. 2;  [ 

:=  p05 . 4 ;  i 

REAL* A 
REA  'P 

QA  8007  ] 
JTICa.2) 

8010  1 

CYCLE 
pGO, 
pi  4 

93  i 

pl  0  :=  plS 

:=  pO  5 . 4 ;  [ 

2;  [  REAL' 4  FXA 

REAL'  8  VTICd,  3) 

8008  ! 
8010  ) 

CYCLE 
pOO 
pi  4 

94  > 

:  =  plS. 2;  [ 

:=  pO  5 . 4 ;  [ 

REAL' 4 
REAL'S 

FYA  8009 
RTIC  (1,1) 

] 

8015  ) 

CYCLE 
pOO 
pi  4 

i  9S  1 

plS  >;  E 
:=  p05 . 4 ;  ( 

REAL* 4 
REAL' 8 

FZA  8011 
RTIC  ( 1 , 2 ) 

1 

8015  i 

CYCLE 
plG 
pl  4 

L  96  1 

:  -  plS. 2;  [ 
:=  p05 . 4 ;  [ 

REAL' 4 
REAL'8 

MX A  6012 
RTIC  (1,3) 

’8015  j 

CYCLE 

plO 

[  97  i 

:=  plS.2;  E 

SEAL* 4 

MYA  8013 

] 

CYCLE 
pl  0 

[  98  , 

:=  plS. 2;  [ 

REALM 

MZA  8014 

J 

CYCLE 

Pl5 

[  99  1 

:=  pC  8 . 2 ;  [ 

REAL' 4 

VRWM(l)  8000  1 

CYCLE 

pis 

[  100  ] 

:=  p08 . 2 ;  [ 

REALM 

VRWM ( 2 )  8000  i 

CYCLE 

plS 

[  101  ] 

:  -  pC  8  .  2  ;  [ 

REAL*  4 

VRWM ( 3 )  8000  ! 

CYCLE 

pis 

[  102  ! 

:=  p  0  8  .  2  :  E 

REALM 

MVRWM  800S  | 

CYCLE 
pl  4  , 

[  103  ! 
p20  :=  pC  5 

.2;  R 

SAL' 4  MAGR 

TR  9000 

CYCLE 

Pl-' 

[  104  ; 

:=  p  0  5 . 4 ;  [ 

REAL'8 

LAMDXX (1) 

9001  ] 

CYCLE 
pl  4 

[  1 05  ) 

:=  pO  5 . 4 ;  [ 

REAL'S 

LAMDXX (2) 

9001  ] 

CYCLE 

p20 

[  106  ! 

:=  pC  5 . 2 ;  [ 

REAL*  4 

LAMSEK ( 1 ) 

9002  ] 

CYCLE 

p20 

r  10  7  1 
:  =  p05.2-  [ 

REAL' 4 

LAMSEK (2) 

9002  1 

C  vc  LE 

'  pis 

;  10  8  ; 

p  0  4 . 2 ;  t 

REAL* 4 

PULSES  (1) 

100CC  ’ 

CYCLE 
pl  3 

:  109  ; 

p  0  4 . 2 ;  : 

REAL' 4 

P’JuSEG  (2) 

10C00  j 

CYCLE 

pl3 

'  no  ; 

:=  pC4.2;  [ 

REALM 

PULSEG (3) 

10000  1 

CYCLE 
P'S  2 

p26 

t  111  j 

:  -  puS  .  2; 

:=  p2  5 . 2 ;  [ 

REAL *4 
REALM 

RRELTR  (1 ) 
BFXACS  11 

1  1000  s 

003  I 

□  02 

p2  6 

l  112  ] 

:*  pCS  .  2  ;  [ 

:  ~  p  2  5 . 2 ;  [ 

REAL' 4 
REALM 

RRELTR 12) 
BFYACS  11 

11000  ] 
0  0  4 
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CYCLE 
p02 
p2  6 

t  113  ] 

:=  P05 . 2;  [ 

:=  p25.2;  [ 

REAL*  4 
REAL* 4 

RRELTR ( 3 )  11000 
BFZACS  11005  ] 

CYCLE 

p02 
p2  6 

[  114  ! 

:=  pOS.2;  [ 
:=  P25.2;  [ 

REAL* 4 
REAL* 4 

VRELTR  1 1 )  110C1 
BMXAC5  11006  J 

CYCLE 

p02 

p26 

[  115  J 
:=  p05 . 2;  [ 

P25.2;  [ 

REAL* 4 
REAL* 4 

VRELTR ( 2 )  11001 
BMYACS  11007  ] 

CYCLE 

p02 
p2  6 

[  116  1 
:»  pOS.2;  [ 
:=  p25 . 2 ;  [ 

REAL* 4 
REAL* 4 

VRELTR ( 3 )  11001 
BMZACS  11008  i 

CYCLE 

p02 
p2  6 

[  1U  ] 

:=  p05 . 2;  [ 

:=  P25 . 2;  i 

REAL* 4 
REAL* 4 

TGOTR  110C2  ) 
BMDOTA  11009  ] 

CYCLE 

pl9 

[  118  1 
:=  p26.1;  [ 

INTEGER*2  IACSONA  11011 

CYCLE 

pl9 

pOO, 

[  119  1 
:=  p25.1;  [ 

plO  :=  p26 

INTEGER*2  IACSONB  11010 
.2;  [  REAL*  4  FXACS  11012 

CYCLE 

pOO 

C  120  ] 

:=  p2  6 . 2 ;  f 

REAL*  4 

FYACS  11013  j 

CYCLE 

pOO 

;  121  i 

: p26 . 2;  [ 

REAL*  4 

FZACS  11014  ] 

CYCLE 

plQ 

[  122  ] 

: =  p2  6 . 2 ;  [ 

REAL*  4 

MXACS  11015  ; 

CYCLE 

plO 

!  123  ] 

:=  P2  6 . 2 ;  [ 

REAL*  4 

MYACS  11016  1 

CYCLE 

plO 

[  124  ] 

: -  p2  6 .2;  [ 

REAL*  4 

MZACS  11017  J 

CYCLE 

pOO 

(  125  ] 

:=  P2  6 . 2 ;  [ 

REAL* 4 

MDOTA  11018  ) 

CYCLE 

pOO, 

[  126  1 
plO  :=  pC  9 

.2;  [  REAL* 4  FXVCS  11019 

CYCLE 

pOO 

[  127  | 

:  =  p09.2;  [ 

REAL*  4 

FYVCS  11020  i 

CYCLE 

pOO 

!  128  ] 

:=  pO  9 . 2 ;  [ 

REA! *4 

FZVCS  11021  1 

CYCLE 

plO 

[  129  ] 

pO  9 . 2 ;  [ 

REAL*  4 

MXVCS  11022  | 

CYCLE 

plO 

[  130  ] 

:*  pO  9 . 2 ;  [ 

REAL* 4 

MYVCS  11 023  | 

CYCLE 

plO 

[  131  1 
:=  pO  9 . 2 ;  [ 

REAL*  4 

MZVCS  11024  1 

CYCLE 

pOC 

t  132  1 
:=  pO  9 . 2 ;  [ 

REAL*  4 

MDOTV  11025  | 

CYCLE 

pOl 

[  133  1 

pi 3. 2;  ' 

REAL*  4 

AT ( 1 )  12C0C  ; 

CYCLE 

pOl 

[  134  1 
:»  pi  3. 2;  ( 

REAL* 4 

AT ( 2 )  12000  i 

CYCLE 
pO  1 

[  135  J 

pl 3. 2;  [ 

REAL*  4 

AT  <  3 )  i^juU  ] 

CYCLE 
pi  4 

t  136  ; 

:  =  p  1  3 . 4 ; 

REAL* 8 

RMIR(l)  12001  i 

CYCLE 
pi  4 

:  137  ] 

p 1 3 . 4  ;  ; 

REAL "8 

RMIR ( 2 )  12001  ’ 
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CYCLE 
pi  4 

[  138 

1 

pi  3 . 4 

;  [  REAL* 8 

RMIR(3)  1 

2001  : 

CYCLE 

pOl, 

[  139 
p06. 

] 

p21 

:=  p 1 3 . 2 ; 

[  REAL* 4 

RMIR_ ! 1 )  12002  ! 

CYCLE 

pOl, 

[  140 

p06. 

1 

p21 

:=  pl3 . 2; 

[  REAL* 4 

RMIR_<2)  12002  1 

CYCLE 

pOl, 

[  141 

p06. 

1 

p21 

:=  pl 3 . 2 ; 

[  REAL* 4 

RMIR_<3)  12002  ] 

CYCLE 
pl  4 

[  142 

] 

pi  3 . 4 

;  1  REAL* 8 

VMIR(l)  12003  ) 

CYCLE 
pi  4 

[  143 

1 

pl3. 4 

;  [  REAL* 8 

VMIR ( 2 )  12003  1 

CYCLE 
pi  4 

[  144 

i 

pl3 . 4 

;  [  REAL *8 

VMIR ( 3 )  12003  ) 

CYCLE 

pOl, 

[  145 

p06. 

1 

p21 

:=  p 1 3 . 2  ; 

[  REAL*  4 

VMIR_ (1 )  12004  ) 

CYCLE 

pOl, 

[  146 
p06. 

] 

p21 

:=  pl 3 . 2; 

[  REAL* 4 

VMIR_(2)  12004  J 

CYCLE 

pOl, 

[  147 
p06, 

! 

p21 

:=  pl 3 . 2 ; 

[  REAL* 4 

VMIR_(3!  12004  ] 

CYCLE 

pl9. 

l  148 
P21 

] 

pl 3 . 2;  [  REAL* 4  SP  12005  ] 

CYCLE 

p06. 

l  149 
pi  9, 

1 

p21 

:=  pl3 . 2; 

(  REAL *4 

SO  12006  ) 

CYCLE 

p06, 

[  150 
pi  9, 

! 

p21 

:=  pl3 . 2; 

[  REAL* 4 

SR  12007  1 

CYCLE 

pOl, 

[  151 

p06. 

] 

pl4. 

p20,  p21 

:=  pl 3 . 2 

;  [  REAL* 4  TI2M(1) 

12008 

CYCLE 

pOl, 

1  152 

p06. 

] 

pl  4 , 

p20,  p21 

:=  pl 3 . 2 

;  [  REAL* 4  TI2M (2) 

12008 

CYCLE 

pOl, 

[  153 

p06. 

] 

pl  4  , 

p20,  p21 

:=  pl 3 . 2 

;  [  REAL *4  TI2M(3) 

12008 

CYCLE 

pOl, 

[  154 
pO  6 , 

1 

pl  4 , 

p20,  p21 

:=  pl 3 . 2 

;  [  REAL*  4  TI2MI4) 

12008 

CYCLE 

pOl, 

[  155 

p06. 

1 

pl  4 , 

p20,  p21 

:=  pl 3 . 2 

;  l  REAL’ 4  TI2MI5) 

12008 

CYCLE 

pOl, 

1  156 

p06. 

pl  4 , 

p20,  p21 

:  =  pl  3 . 2 

;  (  REAL*  4  TI2MI6) 

12008 

CYCLE 

pOl, 

[  157 
p06, 

1 

pl  4 , 

p20 ,  p21 

:=  pl 3 . 2 

;  [  REAL* 4  TI2M(7) 

12008 

CYCLE 

pOl, 

[  158 

p06. 

J 

pl  4 , 

p20,  p2 1 

:=  pl 3 . 2 

;  [  REAL* 4  TI2M18) 

12008 

CYCLE 

pOl, 

[  159 

p06. 

] 

pl  4 , 

p20,  p2 1 

:=  pl 3 . 2 

;  [  REAL* 4  TI2M19) 

12008 

CYCLE 

pOl, 

(  160 
p2 1 

1 

:  p 1 3 . 2 ;  [  REAL* 4  MVR 

12009  ] 

CYCLE 

p?l 

f  161 

1 

pl  3 . 2 

;  [  REAL *4 

VTT(l)  12010  ] 

CYCLE 

p21 

(  162 

i 

pl 3 . 2 

;  [  REAL* 4 

UTT ( 2 )  12010  J 

CYCLE 

pPl 

[  163 

1 

pl3.2 

;  [  REAL* 4 

VTT ( 3 )  12010  j 

CYCLE 

p21 

f  164 

) 

pl4 .2 

;  (  REAL *4 

URREL(l) 

13000  1 
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CYCLE 

p2 1 

[  165  ] 

:=  pi;.  2;  [ 

REAL* 4 

URREL  ( 2 1  13000  1 

CYCLE 

P  21 

r  i  66  i 
:=  pH. 2;  i 

REAL* 4 

L'RREL  ( 3  )  13000  1 

CYCLE 

p20 

[  167  ] 

:=  pi  4 . 2;  [ 

REAL* 4 

RREL(l)  13001  ] 

CYCLE 

p20 

[  168  ] 

:=  pH.  2;  ( 

REAL*4 

RRELI2)  13001  ] 

CYCLE 

p20 

r  69  1 
•  -  pH.  2;  [ 

REAL*  4 

RREL ( 3 )  13001  1 

CYCLE 

p2C 

[  170  ] 

:=  pi  4. 2;  [ 

REAL*  4 

VREL(l)  13002  J 

CYCLE 

p20 

(  171  1 
:=  pi  4. 2;  [ 

REAL* 4 

VRLL ( 2 )  13002  J 

CYCLE 

p20 

[  172  1 
:=  pi  4. 2;  [ 

REAL* 4 

VREL(3)  13002  ! 

CYCLE 
pi  9 

(  17  3  ] 

:=  pi  4 . 2;  [ 

REAL* 4 

TGO  13003  1 

CYCLE 

p21 

[  174  ] 

:=  pi  4 . 2;  [ 

REAL* 4 

MAGR  13004  ] 

CYCLE 

pl9. 

[  175  ] 
p21  :=  pi  4 

.2;  [  REAL*  4  MAGV  13005  1 

CYCLE 

p21 

[  176  ] 

:=  pi  4 . 2 ;  [ 

REAL* 4 

PITERO  13006  ] 

CYCLE 

p21 

[  177  ] 

:=  p 1 4 . 2 ;  ( 

REAL* 4 

YAWER0  13007  ) 

CYCLE 

pl9. 

f  178  ] 

p20,  p21  := 

pH.  1, 

:  [  INTEGER *2  ACQD 

13008 

CYCLE 
pO  6 

[  179  1 

:=  p01.2;  [ 

REAL* 4 

THTER  14000  ] 

CYCLE 

p06 

[  180  ] 

:=  p01.2;  [ 

REAL* 4 

PSIER  14001  ] 

CYCLE 
pOO, 
pi  4 

[  181  ] 
p02,  pi  9  :  = 

:=  p20.2;  ( 

p21 . 1 ; 

REAL* 4 

:  [  INTEGER* 2  IDROP 
FRMRAT  15005  ] 

15000 

CYCLE 
pl9 
pi  4 

i  1  o2  1 

p21 . 1 ;  [ 

:=  p20 . 2 ;  r 

INTEGER*2  IBORND  15001  I 
REAL*  4  LAMMO(l)  15006  1 

CYCLE 

Pi  9 

pi  4 

[  183  ] 

:=  p21 . 1;  [ 

:=  p20 . 2 ;  [ 

INTEGER*  2  IBURNM  15002  1 
REAL*  4  LAMMO (2)  15006  ] 

CYCLE 
pi  9 
pl4 

(  184  ] 

:=  p21 . 1 ;  [ 

:=  P20 . 2;  [ 

INTEGER*2  IDMEAS  15003  ] 
REAL* 4  RRELO(l)  15008  ] 

CYCLE 
pi  9 
pi  4 

[  185  ] 

:=  p21 . 2;  [ 

:=  P20 . 2;  [ 

REAL*  4 
REAL*  4 

ADISTT<1,1)  15004 
RRELO ( 2 )  15008  ] 

i 

CYCLE 
pl9 
pi  4 

[  186  1 

p21 . 2;  [ 

:=  P20 . 2;  [ 

REAL* 4 
REAL* 4 

ADISTT(1,2)  15004 
RRELO ( 3 )  15008  ] 

1 

CYCLE 
P1 9 

[  187  ] 

;=  p21 . 2;  [ 

REAL*  4 

ADISTT(1,3)  15004 

] 

CYCLE 
pi  9 

[  188  ] 

:  =  P21 . 2;  [ 

REAL*  4 

ADISTT(2,1)  15004 

1 

CYCLE 
pi  9 

[  189  ] 

:=  p  2 1 .2;  [ 

REAL* 4 

ADISTT<2,2)  15004 

] 

10 
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CYCLE  i 
pi  9 

190  1 

=  p21 . 2;  [ 

REAL*  4 

ALISTT(2,  3)  15004 

CYCLE  [ 
pi  9 

191  ! 

=  p21 . 2;  [ 

REAL'*  4 

ADISTT(3,1)  15004 

CYCLE  [ 
pi  9 

192  1 

-  p2 1.2;  [ 

REAL*4 

ADISTT(3,2)  15004 

CYCLE  [ 
pi  9 

193  1 

=  p21 . 2;  f 

REAL'4 

ADISTT  (3,  3 )  15004 

CYCLE  [ 
pl9 

194  ] 

=  p21 . 2;  [ 

REAL* 4 

A0ISTT(4,1)  15004 

CYCLE  [ 
pi  9 

195  ] 

-  p21 . 2;  [ 

REAL*  4 

ADISTT (4,2)  15004 

CYCLE  [ 
pi  9 

196  ] 

=  p21 . 2;  [ 

REAL* 4 

ADISTT (4,3)  15004 

CYCLE  [ 
P1  9 

197  j 

-  p21 . 2;  [ 

REAL* 4 

VGM(l)  15007  ] 

CYCLE  [ 
pi  9 

198  ) 

=  p21 .2;  [ 

REAL* 4 

VGM (2)  15007  ] 

CYCLE  f 
pl  9 

199  ] 

=  p21 . 2;  [ 

REAL* 4 

VGM (3)  15007  ] 

CYCLE  (  200  1 

pl4,  p21  :=  p20 

2;  (  REAL* 4  SNRO  15009  ] 

CYCLE  [  201  1 

pl 4  :=  p20 . 2;  ( 

p09,  pl 9  :=  p21 

REAL* 4  TI2MO<l)  15010  ] 

1;  [  INTEGER* 2  XVCS  15011 

CYCLE  [ 
pl  4 
p09 
pOl 

202  ) 

=  p20 . 2 ;  ( 
=  p06. 2;  C 
=  p21 .2;  [ 

REAL* 4 
REAL*  4 
REAL* 4 

TI2MO  1 2 )  15010  ] 
CMMD(l)  15012  J 
UVS<1)  15016  ) 

CYCLE  [ 
pl  4 
p09 
pOl 

203  ] 

-  p20 . 2;  [ 

=  p06 . 2;  [ 

-  p21 .2;  [ 

REAL*  4 
REAL* 4 
REAL* 4 

TI2MO ( 3 )  15010  ) 
CMMD { 2  J  15012  ] 

UVS  (2)  15016  J 

CYCLE  [ 
pl4 
p23 
pOl 

204  ] 

=  p20 . 2;  [ 

=  p0  6 . 2 ;  [ 

=  P21.2;  [ 

REAL* 4 
REAL* 4 
REAL* 4 

TI2MOI4)  15010  ] 
VCMD(l)  15013  1 

UVS  (3)  15016  ] 

CYCLE  [ 
pl  4 
P23 
pOl 

205  ] 

=  p20 . 2;  [ 

-  p06 . 2 ;  [ 

=  P21.2;  [ 

REAL* 4 
REAL* 4 
REAL* 4 

TI2MO ( 5 )  15010  ] 
VCMD ( 2 )  15013  ] 

MVS  15017  ) 

CYCLE  [ 
pl  4 
P23 

206  ] 

-  P20 . 2 ;  [ 

=  p06 . 2;  [ 

REAL* 4 
REAL»4 

TI 2MO ( 6)  15010  ] 
VCMD ( 3 i  15013  ! 

CYCLE  [ 
pl  4 
P23 

207  ] 

-  p20 . 2;  t 

=  pO  6 . 2 ;  [ 

REAL *4 
REAL* 4 

TI2MO ( 7 )  15010  J 
VCMD ( 4 )  15013  1 

CYCLE  [ 
pl  4 

P2  3 

208  J 

-  p20 . 2;  [ 

-  p06 . 1 ;  [ 

REAL* 4  TI 2MO ( 8 )  15010  ] 
INTEGER*  2  IFTAB  15014  1 

CYCLE  [ 
pl  4 
P23 

209  ] 

-  P20 . 2 ;  [ 

-  p06 . 2;  [ 

REAL* 4 
REAL* 4 

TI2MO ( 9)  15010  ) 
TFTAB  15015  ] 

CYCLE  t 
pl  4 

210  ] 

-  p20 . 2;  [ 

REAL *4 

VRELO(l)  15018  ] 

CYCLE  I 

pl  4 

211  i 

=  P20.2;  [ 

REAL* 4 

VRELO ( 2 )  15018  j 

CYCLE  [ 
pl  4 

212  ) 

=  p20 . 2;  [ 

REAL* 4 

VRELOI3)  15018  ] 
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CYCLE 
pi  9 

[  213  ] 

:=  p  1 4 . 2 ;  ( 

REAL* 4 

TGIL  16000  ] 

CYCLE 
pi  9 

[  214  ) 

:=  pi  4 . 2 ;  [ 

REAL* 4 

PITER  16001  ) 

CYCLE 
pi  9 

(  215  ] 

:=  pi  4 . 2;  [ 

REAL* 4 

YAWER  16002  ] 

CYCLE 
pi  9 

[  216  1 
:=  pi  4. 4;  [ 

REAL* 8 

LAMD(l)  16003  ) 

CYCLE 
pi  9 

[  219  1 

pi  4. 4;  [ 

REAL*8 

LAMD(2)  16003  ] 

CYCLE 
pi  9 

t  218  ! 

:=  pl 4 . 2;  ( 

REAL* 4 

TRMTGO  16004  ] 

CYCLE 
pi  9 

[  219  ] 

:=  P14 . 2;  [ 

REAL* 4 

TGE1  16005  ) 

CYCLE 
pi  9 

[  220  i 
:=  pl 4 . 2 ;  [ 

REAL* 4 

TGE2AL  16006  ] 

CYCLE 
pi  9 

[  221  ] 

:=  pl 4 . 1 ;  [ 

INTEGER’2  IBURN1  16007  ] 

CYCLE 

p21 

[  222  ] 

:=  pl 4 . 1 ;  [ 

INTEGER*2  ESTATE  16008  ] 

CYCLE 
pi  9 

[  223  1 

:=  p21 . 2;  [ 

REAL* 4 

ROLLER  16009  ] 

CYCLE 

p26. 

[  224  1 
p25  :=  pl 9 

.2;  [  REAL*  4  ACSLEV  17000  ] 

CYCLE 

p26. 

[  225  ] 
p25  :=  pl 9 

.  1;  (  INTEGER*2  ITHRES  17001 

CYCLE 

p09 

(  226  ] 

:=  P19.2;  ( 

REAL* 4 

DTOFFV(l)  17002  ] 

CYCLE 
pO  9 

[  227  1 
:=  P19. 2;  ( 

REAL *4 

DTOFFV (2)  17002  ] 

CYCLE 

p09 

C  228  ] 

:=  P19.2;  [ 

REAL* 4 

DTOFFV (3)  17002  ] 

CYCLE 
pO  9 

(  229  ] 

:=  pl 9 . 2 ;  [ 

REAL* 4 

DTOFFV ( 4 )  17002  ] 

CYCLE 
pO  9 

(  230  ] 

:=  pl 9 . 1 ;  [ 

INTEGER*2  IVTAB  1 ‘’003  ) 

CYCLE 

p09 

(  231  ] 

:=  pl 9 . 2;  [ 

REAL* 4 

TBURNM  17004  ] 

CYCLE 
pO  9 

(  232  ] 

:=■  pl  9  -  2;  ( 

REAL* 4 

TIMONV  17005  I 

CYCLE 
pO  9 

[  233  ] 

:=  pl 9 . 2 ;  [ 

REAL* 4 

TOFFLT(l)  17006  ) 

CYCLE 

p09 

[  234  1 
:=  pl 9 . 2;  [ 

REAL*  4 

TOFFLT (2)  17006  ] 

CYCLE 
pO  9 

[  235  ] 

=  -  P 1 9 . 2 ;  ( 

REAL* 4 

TOFFLT ( 3 )  17006  I 

CYCLE 
pO  9 

[  236  ] 

pl 9 . 2;  [ 

REAL* 4 

TOFFLT  (4)  l-’OOe  ] 

CYCLE 

p09 

(  237  ] 

:=  pl  9 . 2 ;  [ 

REAL* 4 

l'VTAB  17007  ] 

CYCLE 

p2  6 

f  238  I 
:=  p  1 9 . 2 ;  f 

REAL* 4 

DTACSA(l)  17008  1 

CYCLE 

p26 

[  239  ] 

=  =  P 1 9  -  2 ;  [ 

REAL* 4 

DTACSA ( 2 )  17008  ] 
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CYCLE 

p2  6 

'  240  ] 

:=  pi  9.2; 

[  REAL* 4 

DTACSA (3) 

170OS 

CYCLE 

p2  6 

'  241  ] 

:=  pi  9.2; 

[  REAL* 4 

DTACSA (4) 

17008 

CYCLE 

p25 

[  242  ] 

:=  pi  9.2; 

[  REAL* 4 

DTACSB (1) 

17009 

CYCLE 

p25 

:  243  1 
:=  pi  9 . 2; 

[  REAL* 4 

DTACSB (2) 

17009 

CYCLE 

p25 

[  244  ) 

:=  pi  9.2; 

[  REAL* 4 

DTACSB (3) 

17009 

CYCLE 

p25 

[  245  ] 

:=  pi  9.2; 

f  REAL* 4 

DTACSB (4) 

17009 

CYCLE 

L  246  ] 

p26,  p25  :=  pi 9 . 2 ;  f  REAL* 4  TATAB  17010  1 


CYCLE  [  247  ] 

p21  :=  pi 9 . 1 ;  [  INTEGER*2  MIDBRN  17011  ) 

CYCLE  [  248  1 

p21  :=  pi  9.1;  f  INTEGER *2  ICMD  17012  1 

CYCLE  [  249  ] 

p21  :=  pi  9 . 1 ;  [  INTEGER*2  IDIST  17013  ] 


CYCLE  [  250 


pOO, 

pOl,  p04,  p05. 

p06. 

p08. 

p09,  plC 

1,  p2  6 

:=  p02 , 

.1;  [  INTEGER* 

•2 

IEXIT 

17014 

] 

[  poo 

= 

uublkOO. for. 

S 

* 

45, 

R 

= 

25, 

70  J 

[  p01 

= 

uublkOl . for. 

S 

= 

2, 

R 

= 

24, 

26  ] 

[  p02 

= 

uublk02 . for, 

s 

= 

1, 

R 

= 

12, 

13  1 

[  p04 

= 

uublk03. for. 

s 

= 

3, 

R 

= 

13, 

16  J 

(  P05 

uublk04 . for. 

s 

= 

21, 

R 

27, 

48  ] 

(  p06 

- 

uublk05 . for. 

s 

=«= 

8, 

R 

= 

26, 

34  ] 

(  p08 

= 

uublk06.  for. 

s 

tK 

14. 

R 

S 

19, 

33  ] 

[  p 09 

= 

uublk07 . for, 

s 

= 

14, 

R 

= 

29, 

43  ] 

[  plO 

* 

uublk08 . for. 

s 

= 

10, 

R 

= 

37, 

47  ] 

[  p26 

= 

uublk09.  for. 

s 

= 

8, 

R 

* 

18, 

26  ] 

[  pl2 

- 

uublklO . for, 

s 

= 

3, 

R 

2. 

5  ] 

t  pi  3 

* 

uublkll -for. 

s 

= 

31, 

R 

= 

40, 

71  ] 

t  pi  4 

uublkl2. for. 

s 

* 

25, 

R 

= 

47, 

72  1 

[  pi  5 

= 

uublkl3. for. 

s 

= 

8, 

R 

10, 

18  ! 

t  p25 

uublkl4 . for. 

s 

= 

8, 

R 

= 

11, 

19  ] 

f  pi  9 

= 

uublklS. for. 

s 

= 

26, 

R 

= 

43, 

69  ] 

t  P20 

= 

uublkl6. for. 

s 

= 

19, 

R 

= 

20, 

39  ] 

[  p21 

= 

uublkl7 . for. 

s 

= 

25, 

R 

= 

37, 

62  ] 

i  p23 

FILE: 

XD 

YD 

ZD 

=  uublkl8. for,  S  =  15,  R  = 

uuv22. 19g/debug/priority. txt 

12, 

27  ) 

X 

Y 

Z 

P 

Q 

R 

QUAT 

MA~S 

Clh 

UD 

PD 

VD 

QD 

WD 

RD 

GR 

4 

XYZE 

XYZED 


p23 
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* 

CER 

EISP 

CG 

IXX 

IYY 

IZZ 

BFXACS 

BFYACS 

BFZACS 

BMXACS 

BMYACS 

BMZACS 

BMDOTA 

IACSONB 

IACSONA 

FXACS 

FYACS 

FZACS 

MXACS 

MYACS 

MZACS 

MDOTA 

* 

ALT 

GRT 

VTIC 

ATIC 

FXT 

FYT 

FZT 

MXT 

MYT 

MZT 

MDOTT 

# 

PULSEG 

PRESS 

RRELTR 

RHO 

VSND 

FRCX 

MAGRTR 

FRCY 

VRELTR 

FRCZ 

MRCX 

MRCY 

LAMDXX 

MRCZ 

HDOTF 

LAMSEK 

TGOTR 

FXVCS 

FYVCS 

FZVCS 

MXVCS 

MYVCS 

MZVCS 

FXA 

MDOTV 

FYA 

FZA 

MXA 

MYA 

MZA 

MACH 

QA 

* 

VRWM 

MVRWM 

* 

AT 

RMIR 

VMIR 

ST 

SQ 

SR 

TI2M 

# 

MVR 
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VTT 

THTER 

PSIER 

» 

I  DROP 
IBURND 
IBURNM 
XDMEAS 
ADISTT 
IEXIT 
TGO 
MAGV 
VGM 
* 

TGIL 
IVCS 
ROLLER 
PITER 
YAWER 
LAMD 
TRMTGO 
TG  El 
TGE2AL 
IBURN1 
ACQD 
UVS 
MVS 
# 

CMMD 

VCMD 

IFTAB 

TFTAB 

ACS LEV 

ITHRES 

DTOFFV 

IVTAB 

TBURNM 

TIMONV 

TOFFLT 

TVTAB 

DTACSA 

DTACSB 

TATAB 

MIDBRL 

ICMD 

IDIST 


FILE :  uuv22 . 1 9g /debug /process . txt 


pOO  ssblkOO.bl  <null 
pOl  ssblkOl.bl  <null 
p02  ssblk02.bl  uuexo 
p04  ssblk03.bl  <null 
p05  ssblk04.bl  <null 
p06  ssblkOS.bl  <null 
p08  ssblkO 6 . bl  <null 
p09  ssblkOO.bl  <null 
plO  ssblk08.bl  <null 
p26  ssblk09.bl  <nuli 
pl2  ssblk.10.bl  <null 
pi 3  ssblkll.bl  <null 
pl4  ssblkl2.bl  <null 
p!5  ssblkl3.bl  <null 
p25  ssblkl4.bl  <null 
pl9  ssblklS.bl  <null 
p20  ssblkl6.bl  <null 
p21  ssblkl7.bl  <null 
p23  ssblklS.bl  <null 
sequencer  sequencer, 
crossbar  crossbar. bl 


>  ssblkOO.out 

>  ssblkOl.out 
sim.txt  ssblk02.out 

>  ssblk03.out 

>  ssblk04.out 

>  ssblk05.out 

>  ssblk06.out 

>  ssblk07.out 

>  ssblk08.out 

>  ssblk09.out 

>  ssblklO.out 

>  ssblkll.out 

>  ssblkl2.out 

>  ssblkl3.out 

>  ssblkl 4 . out 

>  ssblkl5.out 

>  ssblkl6.out 

>  ssblkl7.out 

>  s  blkl8.out 

bl  <null>  <null> 
<null>  <null> 


FILE:  uuv22. 19g/debug/uublk00. for 


PROGRAM  BLKOC 

IMPLICIT  DOUBLE  PRECISION 
IMPLICIT  DOUBLE  PRECISION 


(A-H) 

(0-2) 


REAL  CEI (9) 

REAL  CG<3) 

DOUBLE  PRECISION  CIE(9) 
REAL  CIM ( 9) 

DOUBLE  PRECISION  DELT 

DOUBLE  PRECISION  DTEPS 

DOUBLE  PRECISION  DTR 

REAL  EISP 

REAL  FRCX 

REAL  FRCY 

REAL  FRCZ 

DOUBLE  PRECISION  FX 

REAL  FXA 

REAL  FXACS 

REAL  FXT 

REAL  FXVCS 

DOUBLE  PRECISION  FY 

REAL  FYA 

REAL  FYACS 

REAL  FYT 

REAL  FYVCS 

DOUBLE  PRECISION  FZ 

REAL  FZA 

REAL  FZACS 

REAL  FZT 

REAL  FZVCS 

DOUBLE  PRECISION  GB(3) 

DOUBLE  PRECISION  GR(3) 

INTEGER  IDROP 

INTEGER  I EXIT 

INTEGER  IMASS 

DOUBLE  PRECISION  IMPLSO 

DOUBLE  PRECISION  IMPULS 

REAL  IXX 

REAL  IYY 

REAL  IZZ 

DOUBLE  PRECISION  LATLP 
DOUBLE  PRECISION  LONGLP 
DOUBLE  PRECISION  MASS 
DOUBLE  PRECISION  MASSO 
REAL  MASS 

DOUBLE  PRECISION  MOOT 
REAL  MDOTA 
REAL  MDOTF 
REAL  MDOTT 
REAL  MDOTV 
DOUBLE  PRECISION  MGR 
DOUBLE  PRECISION  MSSTG2 
DOUBLE  PRECISION  MXYZDD 
DOUBLE  PRECISION  PHI 
DOUBLE  PRECISION  PHIICD 
DOUBLE  PRECISION  PSI 
DOUBLE  PRECISION  PSIICD 
REAL  QUAT ( 4 ) 

DOUBLE  PRECISION  T 
DOUBLE  PRECISION  TBRK 
DOUBLE  PRECISION  TDROP 
DOUBLE  PRECISION  TEMPMASS 
DOUBLE  PRECISION  THT 
DOUBLE  PRECISION  THTICD 
DOUBLE  PRECISION  TSTEP 
DOUBLE  PRECISION  TSTG1 
DOUBLE  PRECISION  TSTG2 
DOUBLE  PRECISION  U 
DOUBLE  PRECISION  UD 
DOUBLE  PRECISION  V 
DOUBLE  PRECISION  VD 
DOUBLE  PRECISION  W 
DOUBLE  PRECISION  WBANF 
DOUBLE  PRECISION  WD 
DOUBLE  PRECISION  WDOTFR 
DOUBLE  PRECISION  WDOTKV 
DOUBLE  PRECISION  WDOTTI 
DOUBLE  PRECISION  WDOTTP 
DOUBLE  PRECISION  WEIGHT 
DOUBLE  PRECISION  WKV 
DOUBLE  PRECISION  WKVO 
DOUBLE  PRECISION  WPFRAC 
DOUBLE  PRECISION  WPFRCO 
DOUBLE  PRECISION  WPROP 


n  o  o  o  o  o 
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DOUBLE  PRECISION  WPR0P1 
DOUBLE  PRECISION  WPR0P2 
DOUBLE  PRECISION  X 
DOUBLE  PRECISION  XD 
DOUBLE  PRECISION  XDD 
REAL  XD_ 

DOUBLE  PRECISION  XMTOF 
DOUBLE  PRECISION  XYZE<3> 

DOUBLE  PRECISION  XYZED  (3) 

DOUBLE  PRECISION  XYZEDD ( 3 ) 

REAL  XYZE_<3) 

REAL  X_ 

DOUBLE  PRECISION  Y 
DOUBLE  PRECISION  YD 
DOUBLE  PRECISION  YDD 
REAL  YD_ 

REAL  Y_ 

DOUBLE  PRECISION  Z 
DOUBLE  PRECISION  ZD 
DOUBLE  PRECISION  ZDD 
REAL  ZD 
REAL  Z_- 

5INCLUDE ( 1 " /INCLUDE /SSBLKOO . DAT ‘ ) 

*LOOP*  PROLOGUE 

*  INITIALIZE  80x87 

CALL  CW87 

*  initialization  of  variables  not  computed  until  end  of  blkOO 


UD 

* 

0.0 

VD 

= 

0.0 

WD 

* 

0.0 

GR(1) 

= 

0.0 

GR  ( 2 ) 

= 

0.0 

GR  (3) 

= 

0.0 

- C 

MISSILE  STATE  INITIALIZATION  MODULE  - C 

- C 

Initialize  integrated  missile  states  C 

C 

- C 

C  MISSILE  MASS  PROPERTIES 

MASS  -  MASSO 
IMPULS  =  IMPLSO 
WPFRAC  =  WPFRCO 
WKV  =  WKVO 

WPROP  =  WPROP1 

C  COORDINATE  TRANSFORMATION  MATRICES 

CALL  MMK (SNGL (-90 . 0*DTR) , 1 , SNGL (LATLP*DTR) , 2, 

SNGL (LONGLP'DTR) , 3, CEI) 

CIE(l)  =  CEI  (1) 

CIE<2)  =  CEI (4) 

CIE ( 3 )  =  CEI (7) 

CIE (4)  =  CEI (2) 

CIE (5)  -  CEI (5) 

CIE (6)  =  CEI (8) 

CIE (7)  =  CEI (3) 

CIE (8)  »  CEI (6) 

CIE (9)  =  CEI (9) 

C  COMPUTE  MISSILE  STATES  *N  INERTIAL  FRAME 

X  -  XYZE(l) *CEI (1)  +  XYZE (2) *CEI (4)  +  XYZE ( 3 ) *CEI ( 7 ) 

Y  -  XYZE(l)  *CEI  (2)  *-  XYZE  (2)  *CEI  (5)  +  XYZE  (  3 )  *CEI  (  8 ) 

Z  «  XYZE (1) *CEI (3)  +  XYZE (2) *CEI { 6)  +  XYZE ( 3 ) *CEI ( 9 ) 

XD  »  XYZED(l) *CEI (1)  +  XYZED (2) *CEI ( 4 )  +  XYZED < 3 > *CEI < 7 ) 

YD  =  XYZED (1) *CEI (2)  +  XYZED (2 ) *CEI ( 5 )  ‘  XYZED ( 3 ) *CEI ( 8 ) 

ZD  »  XYZED (1) *CEI  (  3)  +  XYZED ( 2 ) *CEI  ( 6)  *  XYZED ( 3) *CEI  ( 9) 

XDD  =  XYZEDD (1) *CEI (1)  +  XYZEDD (2) *CEI ( 4 )  *  XYZEDD ( 3 ) *CE I ( 7 ) 

YDD  »  XYZEDD (1 ) *CEI (2)  +  XYZEDD (2) *CEI (5)  *  XYZEDD ( 3 ) *CE I ( 8 ) 


o  n  n  n  n  o 
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ZDD  =  XYZEDD (1) *CEI (3)  +  XYZEDD ( 2 ! *CEI < 6)  +  XYZEDD (3) *CEI (9) 

C  INITIAL  MISSILE  EULER  ANGLES  IN  RADIANS 

PHI  =  PHIICD*DTR 
THT  =  THTICD*DTR 
PSI  =  PSI ICD*  DTR 

C  COMPUTE  INERTIAL  TO  MISSILE  TRANSFORMATION  MATRIX 

CALL  MMK (SNGL (PHI) , 1, SNGL(THT) , 2, SNGL (PSI)  ,  3,CIM) 

C  INITIALIZE  MISSILE  TRUTH  STATES 


CALL 

INTEGI 

(  MASS 

,  MDOT 

,  7  , 

i 

CALL 

INTEGI 

(  WPROP 

,  WDOT7P 

,  7  , 

2 

CALL 

INTEGI 

(  IMPULS 

,  WDOTTI 

,  7  , 

3 

CALL 

INTEGI 

(  WPFRAC 

,  WDOTFR 

,  7  , 

4 

CALL 

INTEGI 

(  WKV 

,  WDOTKV 

,  7  , 

5 

CALL 

INTEGI 

(  XD 

,  XDD 

,  T  f 

6 

CALL 

INTEGI 

(  YD 

,  YDD 

,  T  , 

7 

CALL 

INTEGI 

(  ZD 

,  ZDD 

/  T  , 

8 

CALL 

INTEGI 

(  X 

,  XD 

,  T  , 

9 

CALL 

INTEGI 

(  Y 

,  YD 

,  7  , 

10 

CALL 

INTEGI 

(  Z 

,  ZD 

,  T  x 

11 

C 

C 

C 

C 

C 

C 

C 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
•LOOP*  START 

- - - c 

MISSILE  STATE  UPDATE  MODULE - C 

- c 

Inteqrate  missile  states  to  current  time  C 

C 

- c 

*  tmsudriv  is  no  longer  needed  --  IF/ENDIF  and  assignment  deleted 

*  The  extrapolated  states  have  been  deleted.  There  should  be  no  need 

*  to  look  into  the  future. 

*  Note  that  the  states  which  follow  have  all  been  initialized,  and  each 

*  is  integrated  at  the  end  of  the  timestep. 

XD_  =  SNGL (XD) 

YD_  =  SNGL (YD) 

ZD_  =  SNGL (ZD) 

X_  =  SNGL  (X) 

Y_  =  SNGL (Y) 

Z_  =  SNGL(Z) 

MASS_  =  SNGL (MASS) 

CALL  SEND_REAL_64BIT (  XD  ) 

CALL  SEND_REAL_32BIT (  XD_  ) 

CALL  SEND_REAL_64BIT (  YD  ) 

CALL  SEND_REAL_32BIT (  YD_  ) 

CALL  SEND_REAL_64BIT (  ZD  ) 

CALL  SEND_REAL_32BIT (  ZD_  ) 

CALL  SEND_REAL_64BIT (  X  ) 

CALL  SEND_REAL_32BIT (  X_  ) 

CALL  SEND_REAL_64BIT (  Y  ) 

CALL  SEND_REAL_32BIT (  Y_  ) 

CALL  SEND_REAL_64BIT  (  Z  ) 

CALL  SEND_REAL_32BIT (  Z_  ) 

CALL  RECEIVE_REAL_32BIT (  QUAT(l)  ) 

CALL  RECEIVE_REAL_32BIT (  QUAT(2)  ) 

CALL  RECEIVE_REAL_32BIT (  QUAT(3)  ) 

CALL  RECEI VE_REAL_32BIT (  QUATI4)  ) 

*  MASS  is  much  like  the  other  state  variables  above  in  that  it  should 

*  have  a  very  close  value  at  this  point  in  the  code.  Other  partitions 

*  will  be  notified  one  timestep  later,  however,  about  staging. 

CALL  SEND_REAL_64BIT (  MASS  ) 

CALL  SEND  REAL  32BIT (  MASS  ) 


oooooo  no 
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initialization  of  these 
sent  early. 

CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  S£ND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_648IT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND  REAL  64BIT ( 


variables  was  added  so  that 

CIM(l)  ) 

CIM  ( 2 )  ) 

CIM  ( 3 )  ) 

CIM ( 4 )  ) 

CIM  ( 5 )  ) 

CIM  ( 6)  ) 

CIM  { 7  )  ) 

CIM<8)  ) 

CIM  (  9 )  ) 

UD  ) 

VD  ) 

WD  ) 

GR  (1 )  ) 

GR  ( 2  )  ) 

GR  ( 3 )  ) 


they  could  be 


TRANSFORM  INERTIAL  POSITION,  VELOCITY  AND  ACCELERATION 
TO  EARTH  FRAME 


XYZE ( 1 ) 
XYZE (2) 
XYZE (3) 


CIE (1) *X 
CIE (2) *X 
CIE  (3) *X 


CIE (4) *Y 
CIE  (5) *Y 
CIE (6) *Y 


+  CIE (7) *Z 
+  CIE (8) *Z 
+  CIE (9) *Z 


XYZE_  ( 1 ) 
XYZE_ (2 ) 
XYZE  (3) 


SNGL (XYZE ( 1 ) ) 
SNGL (XYZE (2) ) 
SNGL (XYZE (3) ) 


CALL  SEND_REAL_64BIT (  XYZE(l)  ) 
CALL  SEND_REAL_64BIT (  XYZE (2)  ) 
CALL  SEND_REAL_64BIT (  XYZE (3)  ) 
CALL  SEND_REAL_32BIT (  XYZE_(1)  ) 
CALL  SEND_REAL_32BIT (  XYZE_(2)  ) 
CALL  SEND  REAL  32BIT (  XYZE  (3)  ) 


XYZED(l)  =  CIE  ( 1 J  *XD  +  CIE(4)*YD  +  CIE(7)*ZD 
XYZED ( 2 )  -  CIE (2) *XD  +  CIE(5)*YD  +  CIE(8)*ZD 
XYZED ( 3 )  =  CIE ( 3 ) *XD  +  CIE(6)*YD  +  CIE(9)*ZD 


CALL  SEND_REAL_64BIT (  XYZED (1)  ) 
CALL  SEND_REAL_64BIT (  XYZED (2)  ) 
CALL  SEND  REAL  64BIT (  XYZED (3)  ) 


MASS  PROPERTIES  MODULE  - 

Update  mass  flow  rate,  eg  and  inertia 


CALL  MASSPR (T, MDOTT, MDOTF, MDOTA, MDOTV, MASS, EISP, TBRK, IMASS, 
MDOT, WEIGHT, WDOTTP, WDOTFR, WDOTKV, WDOTTI , CG, IXX , 
IYY, IZZ) 


CALL  SEND_REAL_32BIT (  EISP  ) 
CALL  SEND_REAL_32BIT (  CG(1)  ) 

CALL  SEND_REAL_32BIT (  CG<2)  ) 
CALL  SEND_REAL_32BIT (  CG<3)  ) 
CALL  SEND_REAL_32BIT (  IXX  ) 
CALL  SEND_REAL_32BTT (  IYY  ) 
CALL  SEND  REAL  32BIT (  IZZ  ) 


moved  up  here,  since  MISSIL  doesn't 
the  old  MASS  value,  which  is  saved) 
TEMPMASS  =  MASS 


generate  these  derivs  (it  needs 


CALL  INTEG 
CALL  INTEG 
CALL  INTEG 
CALL  INTEG 
CALL  INTEG 


MASS 

WPROP 

IMPULS 

WPFRAC 

WKV 


MDOT 

WDOTTP 

WDOTTI 

WDOTFR 

WDOTKV 


C  from  BTHRST 

CALL  RECEIVE_REAL_32BIT (  FXT  ) 
CALL  RECEIVE_REAL_32BIT (  FYT  ) 
CALL  RECEIVE_REAL_32BIT (  FZT  ) 
CALL  RECEIVE_REAL_32BIT (  MDOTT  ) 
C  from  FRCTHR 


n  o  n  n  n  o 
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c 


c 


CALL  RECEIVE_REAL_32BIT{  FRCX  ) 
CALL  R£CEIVE_REAL_32BIT (  FRCY  ) 
CALL  RECEIVE_REAL_32BIT (  FSCZ  ) 
CALL  RECEIVE_REAL_32BIT (  MDOTF  ) 
from  AERO 

CALL  RECEIVE_REAL_3?3IT (  FXA  ) 
CALL  RECEIVE_REAL_32BIT (  FYA  ) 
CALL  RECEIVE_REAL_32BIT (  FZA  ) 
from  ACSTHR 

CALL  RECEIVE_REAL_32BIT (  FXACS  ) 
CALL  RECEIVE_REAL_32BIT (  FYACS  ) 
CALL  RECEIVE_R£AL_32BIT (  FZACS  ) 
CALL  RECEIVF_REAL_32BIT (  MDOTA  ) 
from  VCSTHR 

CALL  RECEIVE_REAL_32BIT (  FXVCS  ) 
CALL  RECEIVE_REAL_323IT (  FYVCS  ) 
CALL  RECEIVE_REAL_32BIT {  FZVCS  ) 
CALL  RECEIVE  REAL  32BIT (  MDOTV  ) 


C 

C 

c 

c 

c 


VEHICLE  STATES  MODULE 


Compute  missile  state  derivatives 


C 

■C 

c 

c 

c 

■c 


CALL  MISSLT (T, QUAT, CIM, TEMPMASS,  FXA,  FXT, 

FRCX, FXACS, FXVCS, FYA, FYT, FRCY, FYACS, FYVCS, FZA, 
FZT, FRCZ, FZACS, FZVCS, 

X,  Y,  Z,  XD,  YD,  ZD,  UD,  VD,  WD, 

GB,  GR,  MGR,  FX,  FY,  FZ,  XDL  ,  YDD,  zDD,  MXYZDD, 

U,V,W, PHI, THT, PSI) 


C 

c 

c 

C 

C 

C 

c 

c 

C 

c 

c 

C 

V- 

C 

c 


- c 

MISSILE  STATE  INTEGRATION  MODULE  C 

- c 

Revise  missile  states  using  derivatives  C 
just  computed  .  Missile  states  must  not  C 
be  integrated  if  a  cable  lookup  index  C 

transition  has  occurred  since  the  last  C 
integration  step  .  The  next  integration  C 
step  should  be  rescheduled  to  coincide  C 

with  the  earliest  detected  table  lookup  C 
index  transition  instead  .  Otherwise  C 

schedule  the  next  integration  step  to  C 

occur  at  the  default  step  size  .  C 

C 


TRAPEZOIDAL  INTEGRATION  FOR  SIMPLICITY 


CALL 

INTEG 

XD 

,  XDD 

T 

6 

CALL 

INTEG 

YD 

,  YDD 

1  T 

7 

CALL 

INTEG 

ZD 

,  ZDD 

,  T  , 

8 

CALL 

INTEG 

X 

,  XD 

T 

9 

CALL 

INTEG 

Y 

,  yd 

'  T  ! 

10 

CALL 

INTEG 

Z 

,  ZD 

,  T  , 

11 

C 

C 

C 


SEPARATION  MODULE 


Models  discontinuities  occurir.g  during 
stage  separation 


FIRST  STAGE  SEPARATION 

IF  (  DABS (T-TSTG1 ) . LE . DTEPS  )  THEN 
MASS  -  MSSTG2 

WPROP  =  WPPCP2 

IMASS  =  1 


REINITIALIZE  PERTINEN 


INTEGRALS 


o  o  n  o  o  o  o 
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CALL 

INTEGI 

MASS 

O.ODO 

T 

] 

CALL 

INTEGI 

WPROP 

C.GDO 

T 

2 

CALL 

INTEGI 

IMPULS 

O.ODO 

T 

3 

CALL 

INTEGI 

WKV 

O.ODO 

T 

5 

END  IF 

SECOND  STAGE  SEPARATION 

IF  (  DABS (T-TSTG2) . LE.DTEPS  )  THEN 
MASS  =  WKV  /  XMTOF 
WPROP  =  0 . C 
IMPULS  =0.0 
I, -IASS  =  1 

REINITIALIZE  PERTINENT  INTEGRALS 


CALL  INTEGI 
CALL  INTEGI 
CALL  INTEGI 
CALL  INTEGI 
ENDIF 


(  MASS  ,  0.0D0 
(  WPRoP  ,  O.CDC 
(  IMPULS  ,  O.ODO 
(  WKV  ,  O.ODO 


1  ) 

2  ) 
3  ) 
5  > 


NOSE  FAIRING  /  BOOST  ADAPTER  SEPARATION 


IF  (  I  DROP . EQ . 1  .OR.  ( DABS (T-TDROP )  . LE . DTEPS)  )  THEN 
WKV  =  WKV  -  W3ANF 

MASS  =  WKV /XMTOF 


REINITIALIZE 

PERTINENT 

INTEGRALS 

CALL  INTEGI 

(  MASS 

O.ODO  , 

T  , 

1  ) 

CALL  INTEGI 

(  WPROP  , 

O.ODO  , 

T  , 

2  ) 

CALL  INTEGI 

(  IMPULS  , 

O.ODO  , 

T  , 

3  ) 

CALL  INTEGI 

<  WKV 

O.ODO  , 

T  , 

5  ) 

ENDIF 

CALL  RECEIVE  SIGNED  I6BIT1  IDROP  ) 


TERMINATION  LOGIC 


tfires  the  simulation  termination 
conditions 


C  increment  time 

TSTEP  =  TSTEP  +  1 . 0D0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MOPE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_5IGNED_I6BIT (  IEXIT  ) 

' LCcp  *  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE:  uuv22.19q/debug/uublk01. for 


PROGRAM  BLK01 

IMPLICIT  REAL 
’MPLICIT  REAL 

REAL  AC  {  3) 
REAL  AT  (3) 
REAL  DELT 
REAL  DT 
REAL  DTBGU 


(A-H) 
(O-  Z) 


n  n  n  o  n  n  n 
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REAL  DTMP1 
REAL  DTR 
REAL  GMU 
REAL  GREST (3) 

INTEGER  IEXIT 
INTEGER  IMINSF 
REAL  KA 
REAL  KV 
REAL  MVR 
REAL  MVS 
REAL  PG  (3) 

REAL  PGO ( 3 ) 

REAL  PGD<3) 

REAL  PM (3) 

REAL  PSI 
REAL  PSIER 
REAL  PSIICD 
REAL  PSIPG 
REAL  RADE 
REAL  RMIR_(3) 

REAL  SPSI 
REAL  STHT 
REAL  T 
REAL  T5 
REAL  TFRCS 
REAL  TGCALL 
REAL  TGPUDRIV 
REAL  TGPUSTEP 
REAL  THT 
REAL  THTER 
REAL  THTICD 
REAL  THTPG 
REAL  TI2MI9) 

REAL  TIMTMP 
REAL  TLGPU 
REAL  TMP1 
REAL  TMP  2 
REAL  TMP  3 
REAL  TMP 4 
REAL  TMP 5 
REAL  TSTCAL 
REAL  TSTEP 
REAL  TSTG2 
REAL  US ( 3 ) 

REAL  USO (3) 

REAL  USOD 
REAL  USD (3) 

REAL  USF ( 3 ) 

REAL  USFD 
REAL  USI (3) 

REAL  UVS { 3 ) 

REAL  VELWO 
REAL  VELWD 
REAL  VELWST 
REAL  VFRCS 
REAL  VGEMS 
REAL  VMIR_ (3) 

REAL  VRATIO 
REAL  VW  (3) 

REAL  VWD  (3) 

REAL  VWIC (3) 

REAL  WASTAN 
REAL  WC ( 3 ) 

$  INCLUDE  ( '  '•/INCLUDE/SSBLK01  .CAT' 

♦LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW8  7 


C 

C- 

C 

C 

C 


MISSILE  STATE  INITIALIZATION  MODULE 
Initialize  integrated  missile  states 


C 


INITIAL  MISSILE  EULER  ANGLES  IN  RADIANS 


non 


22 


Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1,  Part  3 


THT  =  THTICD'DTR 
PSI  =  PSIICD*DTR 

C  ESTIMATED  MISSILE  EULER  ANGLES  AND  BODY  RATES 

STHT  =  THT 
SPSI  =  PSI 

C  INITIALIZE  NAVIGATION  INTEGRATED  PARAMETERS 

VELW5T  -  VELWO 
VW  (1)  =  VWIC  (1) 

VW<2>  »  VWIC  (2) 

VW  (3)  =  VWIC  (3) 

C  INITIAL  UNIT  STEERING  VECTOR 

USO(l)  =  COS (SPSI) *COS (USOD’DTR) 

USO (2)  -  SIN (SPSI) *COS (USOD'DTR) 

USO (3)  =  -SIN (USOD'DTR) 

C  FINAL  UNIT  STEERING  VECTOR 

USF(l)  =  COS (SPSI) *COS (USFD*DTR) 

USF (2)  »  SIN (SPSI ) *COS <USFD*DTR) 

USF ( 3 )  =  -SIN (USFD'DTR) 

INTERMEDIATE  UNIT  STEERING  VECTOR  (  AT  FRACS  INITIATION  ) 
ESTIMATE  DELTA  FLTGHT  PATH  ANGLE  DURING  MINS  PORTION  OF 
FRACS  DUE  TO  GRAVITY 

TIMTMP  =  T5 

USI(l)  =  USF  ( 1 ) 

USI  (2)  =  USF (2) 

USII3I  =  USF  ( 3 ) 

GREST(l)  =  -  GMU/RACE**2 
GREST ( 2 )  =  0.0 

GREST (3)  -  0.0 

5  CONTINUE 

TMP1  «  GREST (2) *USI (3)  -  GREET (3 ) *USI ( 2 ) 

TMP2  -  GREST (3) *USI (1)  -  GREST (1) *USI (3) 

TMP3  =  GREST (X) *USI (2)  -  GREST ( 2 ) *USI ( 1 ) 

TMP  4  =  (  TIMTMP  -  TFRCS  )/(  T5  -  TFRCS  ) 

TMP  5  =  VFRCS  +  TMP  4* (  VGEMS  -  VFRCS  ) 

USD  (1 )  =  (  USI(2)*TMP3  -  USI(3)*TMP2  )  /TMP5 
USD (2)  =  (  USI ( 3 ) * TMP 1  -  USI(1)*TMP3  ) /TMP5 
USD (3)  =  (  USI(1)*TMP2  -  USI(2)*TMP1  ) /TMP 5 
TIMTMP  -  TIMTMP  -  DTBGU 
USI  (I)  =  USI(l)  -  DTBGU* USD  ( 1 ) 

USI (2)  =  USI (2)  -  DTBGU*USD (2) 

USI (3)  =  USI (3)  -  DTBGU*USD(3) 

TMP1  =  SQRT  (  USI (1 ) * *2  +  USI (2) **2  +  USI(3)**2  ) 

USI (1)  =  USI (1) /TMP1 
USI  (2)  =  USI (2) /TMP 1 
USI  (3)  =  USI (3) /TMP1 
IF  (  TIMTMP. GT. TFRCS  )  GO  TO  5 

C  INITIALIZE  GUIDANCE  INTEGRATED  PARAMETERS 

PG0(1)  »  COS (SPSI) *COS (STHT) 

PGO ( 2 )  *  SIN (SPSI) *COS (STHT) 

PGO  (3)  =  -SIN (STHT) 

US(1)  =  US0(1) 

US  (2)  =  USO (2) 

US ( 3 )  *  USO ( 3 ) 

PG ( 1 )  =  PGO ( 1 ) 

PG  (2)  «  PGO (2) 

PG  (3)  =  PGO (3) 


C 

c 

c 

c 

c 

c 

c 


- c 

MAIN  EXECUTION  LOOP  - C 

- „_c 

Execution  of  all  events  is  performed  C 

within  this  loop  C 

C 

-  - c 


n  o 
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1000  CONTINUE 
•LOOP*  START 

- c 

ON  BOARD  GUIDANCE  PROCESSING  C 

- c 

Determine  guidance  commands  C 

- C 


IF  (  TSTEP  .GE.  TGPUDRIV  )  THEN 

*  TGPUDRIV  -  TGPUDRIV  +  TGPUSTEP 

C  GET  TIME  SINCE  LAST  GUIDANCE  PROCESSOR  UPDATE 

C  DT  =  T  -  TLGPU 

TLGPU  »  T 

DT  =  TGPUSTEP  *  DELT 

C  INTEGRATE  GUIDANCE  STATES  FROM  LAST  PASS  THROUGH 

US  (1 )  »  US(1)  +  DT*USD  ( 1 ) 

US (2)  =  US { 2 )  +  DT*USD(2) 

US  (3)  =  US  ( 3 )  +  DT*USD(3) 

VELWST  =  VELWST  +  DT'VELWD 

PG  (1)  =  PG (1)  +  DT'PGD  (1) 

PG  <  2 )  =  PG  (2 )  +  DT*PGD(2) 

PG  (3)  =  PG  (3)  +  DT*PGD  (3) 

VW(1)  =  VW(1)  +  DT*VWD(1) 

VW ( 2 )  =  VW (2)  +  DT*VWD(2) 

VW ( 3 )  =  VW (3)  +  DT’VWD(3) 

C  NORMALIZE  UNIT  STEERING  VECTOR 

DTMP1  =  SQRT  (  US ( 1 ) * *2  +  US ( 2 ) * ” 2  *  US(3)**2  ) 

US (1)  =  US  (1 )  /  DTMP1 

US  (2)  =  US  (2)  /  DTMP1 

US (3)  =  US (3)  /  DTMP1 

C  NORMALIZE  UNIT  POINTING  VECTOR 

DTMP1  =  SQRT  (  PG(1)**2  +  PG(2)**2  +  PG(3>**2  ) 

PG ( 1 )  =  PG  <1 )  /  DTMP1 

PG ( 2 )  =  PG  (2 )  /  DTMP1 

PG ( 3 )  =  PG  <3 )  /  DTMP1 

C  DETERMINE  COMMANDED  BODY  ANGLES  FOR  OUTPUT  COMPARISON 

THTPG  =  -  ASIN  (  PG ( 3 )  ) 

PSIrG  -  ATAN2  (  PG  < 2 )  ,  PG(1)  ) 

ENDIF 

CALL  RECEIVE_REAL_32BIT (  AT (1 )  ) 

CALL  RECEIVE_REAL_32BIT (  AT (2)  ) 

CALL  RECEIVE_REAL_32BIT (  AT (3)  ) 

CALL  RECEIVE_REAL_32BIT (  RMIR  (1)  ) 

CALL  RECEIVE_REAL_323IT (  RMIR_(2)  ) 

CALL  RECEIVE_REAL_32BIT (  RMIR_<3)  ) 

CALL  RECEIVE_REAL_32BIT (  VMIR_U)  ) 

CALL  RECEIVE_REAL_32BIT (  VMIR_(2)  ) 

CALL  RECEIVE_REAL_32BIT (  VMIR_(3)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2MI1)  ) 

CALL  RECEIVF._REAL_32BIT  (  TI2MI2)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2MI3)  ) 

CALL  RECEI VE_REAL_32BIT  (  TI2MM)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2M(5)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2M<6)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2M(7)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2MI8)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2MI9)  ) 

CALL  RECEIVE_REAL_32BIT (  MVR  ) 

- c 

BOOST  STEERING  MODULE  - C 


o  o 
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c - c 

C  Calculates  the  unit  steering  and  C 
C  acceleration  direction  vector  for  boost  C 
C  phase  steering  C 
C  C 
c - C 


IF  (  TSTEP  .GE.  TGPUDR1V  )  THEN 

TGPUDRIV  =  TGPUDRIV  +  TGPUSTEP 

IF  (  T.GE.TSTCAL  -AND.  T.LT.TSTG2  )  THEN 

CALL  BSTEER  <T,  USI, USF,  UVS,  MVS,  MVR,  AT,  RMTR__,  VMIR_,  US,  USD, 
AC, WASTAN, VRATIO, VELWQ) 


c - c 

c - BOOST  GUIDANCE  MODULE - C 

C - c 

C  This  code  calculates  the  error  between  C 

C  the  commanded  pointing  vector  and  the  C 

C  actual  direction  in  whic1'  Lne  intercep-  C 

C  tor  is  pointing.  This  error  signal  is  C 

C  then  sent  to  th°  autopilot.  C 

C  C 

C -  C 


CALL  BGUID (T. A", AC, TI2M, PC, IMINSF, VW, PGD, VWD, WC, PSIER, 
THTER, PM, KA, XV) 

C  SCHEDULE  TIME  FOR  NEXT  BOOST  STEERING/GUIDANCE  CALL 

DTMP1  =  DTBGU  *  ANINT  (  (T+DTBGU)  /  DTBGU  ) 

TSTCAL  =  DTMP1 

TGCALL  =  DTMP1 

ENDIF 

ZERO  BOOST  STEERING/GUIDANCE  DERIVATIVES  AFTER  SECOND  STAGE 
SEPARATION 

IF  (  T.GE.TSTG2  )  THEN 


USD (1 ) 

= 

0.0 

USD (2) 

= 

0.0 

USD (3) 

* 

0.0 

PGD (1 ) 

= 

0.0 

PGD  (2) 

- 

0.0 

PGD ( 3  J 

= 

0.0 

VWD (1) 

= 

0.0 

VWD  (2) 

= 

0.0 

VWD  (3) 

= 

0.0 

ENDIF 

ENDIF 

CALL  SEND_REAL_32BIT (  THTER  ) 

CALL  SEND_REAL  32BIT (  PSIER  ) 

CALL  RECEIVE_REAL_32BIT (  UVS(l)  ) 
CALL  RECEIVE_REAL_32BIT (  UVS (2)  ) 
CALL  RECEIVE_R£AL_32BIT (  UVS (3)  ) 
CALL  RECEIVE  REAL  32BIT (  MVS  ) 


C - c 

c - TERMINATION  LOGIC - C 

c - c 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

c - C 

C  increment  time 

TSTEP  =  TSTEP  +  1.0 
T  *  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  RECEIVERS IGNED_1 6BIT (  IEXIT  ) 
•LOOP*  STOP 
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IF  (  IEXIT.EQ.0  )  GO  TO  1000 
‘LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 1 9g/debug/uublk02 -for 


PROGRAM  BLK02 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

$ INCLUDE ( 1 :PFP: INCLUDE/TARGET. FOR' ) 

REAL  ALT 
REAL  DELT 
REAL  DTEPS 
REAL  DTPRT 
INTEGER  I 
INTEGER  IDROP 
INTEGER  IEXIT 
INTEGER  MESSAGE_SIZE 
INTEGER  M£SSAGE_TYPE 
REAL  MISS 

INTEGER  NUMBER_OUTPUT 
REAL  OUTPUT (5,  0:149) 

REAL  RRELTR ( 3 ) 

REAL  T 
REAL  TDROP 
REAL  TFINAL 
REAL  TGOMN 
REAL  TGOTR 
REAL  TSTEP 
REAL  TSTG1 
REAL  TSTG2 
REAL  VRELTR (3) 

REAL  X_ 

REAL  Y 
REAL  Z~ 

SINCLUDE ( 1 " /INCLUDE /SSBLK 02 . DAT ' ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87  • 

CALL  CW87 

CALL  INPUT_MESSAGE (  MESSAGE_TYPE,  TFINAL,  MESSAGE_SIZE  ) 


C 

C 

C 

C 

C 

C 

C 


- c 

MAIN  EXECUTION  LOOP - C 

- C 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- C 


1000  CONTINUE 
•LOOP*  START 

CALL  RECEIVE 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE' 
CALL  RECEIVE' 


REAL_32BIT (  X_  ) 
'REAL  32BIT  (  Y_  ) 
"REAL~32BIT  (  Z_  ) 
REAL_32BIT (  ALT  ) 
REAL_32BIT (  RRELTR ( ] 
REAL_32BIT (  RRELTR C 
REAL_32BIT (  RRELTR C 
REAL_32BIT (  VRELTR C 
'REAL_32BIT(  VRELTR  C 
REAL_32BIT (  VRELTR C 
REAL_32BIT  t  TGOTR  ) 
'SIGNED  16BITI  IDROP 


) 

) 

) 

! 

) 

) 


C 

C 


SEPARATION  MODULE 


C 

C 


on  no  nnnnnonon  nnnnoon  o  o  n  onnno 
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Models  discontinuities  occuring  during 
stage  separation 


FIRST  STAGE  SEPARATION 

IF  (  ABS (T-TSTG1) .LE.DTEPS  )  THEN 
CALL  OUTMES (010) , T, 0. 0) 

ENDIF 

SECOND  STAGE  SEPARAiION 

IF  (  ABS (T-TSTG2) .LE.DTEPS  )  THEN 
CALL  OUTMES (0102, T, 0. 0) 

ENDIF 

NOSE  FAIRING  /  BOOST  ADAPTER  SEPARATION 

IF  (  I DROP . EQ. 1  .OR.  (ABS (T-TDROP ) . LE. DTEPS)  )  THEN 
CALL  OUTMES (0103,  T, 0. 01 
ENDIF 


- c 

OUTPUT  MODULE - C 

- C 

Creates  print  and  plot  output  data  C 

files  C 

C 

- c 


if  (  nint (mod (tstep, dtprt) ) .eq. 0  )  then 
CALL  OUTMES (0104 , T, ALT) 

OUTPUT (l,NUMBER_OUTPUT)  =  T 
OUTPUT (2, NUMBER_OUTPUT)  =  ALT 
OUTPUT (3, NUMBER_OUTPUT)  -  X_ 

OUTPUT (4, NUMBER_OUTPUT)  =  Y_ 

OUTPUT (5, NUMBER  OUTPUT)  =  Z_ 
NUMBER_OUTPUT  =~NUMBER_OUTPUT  +  1 
ENDIF 


- c 

-  TERMINATION  LOGIC  - C 

- c 

Defines  the  simulation  termination  C 

conditions  C 

C 

- c 

ENABLE  EXIT  IF  INTERCEPT  HAS  OCCURRED  AND  ALL  EVENTS  SCHEDULED  FOR 

THIS  TIME  HAVE  BEEN  EXECUTED 

IF  (  (TGOTR . LE . TGOMN  )  .AND.  (T.GT.1.0)  ) THEN 
IEXIT  =  1 
ENDIF 

ENABLE  EXIT  IF  MAXIMUM  SIMULATION  TIME  HAS  BEEN  EXECUTED  AND  ALL 
EVENTS  SCHEDULED  FOR  THIS  TIME  HAVE  BEEN  EXECUTED 

IF  (  T.GE.TFINAL  )  THEN 
IEXIT  -  1 
ENDIF 

ENABLE  EXIT  IF  MISSILE  HAS  IMPACTED  AND  ALL  EVENTS  SCHEDULED  FOR 
THIS  TIME  HAVE  BEEN  EXECUTED 

IF  (  ALT. LT. 0.0  )  THEN 
IEXIT  =  1 
ENDIF 

C  increment  time 

TSTEP  =  TSTEP  +  1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  SEND  SIGNED  16BITI  IEXIT  ) 


o  o  o  o  o 


nnonn 
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•LOOP*  STOP 

IF  (  IEXIT.EQ.0  )  GO  TO  1000 


C - C 

C - POINT  OF  CLOSEST  APPROACH  CALCULATION  — C 

C - c 

C  Determines  the  miss  distance  at  the  C 

C  point  of  closest  approach  C 

C  C 

C - C 


MISS  -  SQRT  (  (RRELTR(l) 
+  (RRELTft (2) 
+  (RRELTR (3) 

CALL  OUTMES (0105, T, MISS) 

OUTPUT (1, NUMBER_OUTPUT) 
OUTPUT (2, NUMBER_OUTPUT) 
OUTPUT (3, NUMBER_OUTPUT) 
OUTPUT (4, NUMBER_OUTPUT) 
OUTPUT (5, NUMBERJOUTPUT) 


+  VRELTR ! 1 ) *  TCCTR )  ‘  *  2 
+  VRELTR (2) *TGOTR) **2 
+  VRELTR (3) *TGOTR) **2  ) 


=  T 

=  MISS 
=  X_ 

-  Y_ 

=  Z 


- c 

Creates  print  and  plot  output  data  C 

files  C 

C 

- c 


DO  500  1=0,  NUMBERJOUTPUT 

CALL  OUTPUT_MESSAGE  (%VAL  (REAL_32BIT)  ,0'JTPUT  (1,1),  %VAL  (INT2  (5)  )  ) 
CALL  OUTPUT_NL 
500  CONTINUE 

•LOOP*  EPILOGUE 

END 


FILE:  uuv22 . 1 9g/debug/uublk03 . for 


PROGRAM  BLK03 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  CIM  ( 9; 

. EAL  DELT 
INTEGER* 4  GYSEED 
INTEGER  IEXIT 
REAL  P 

REAL  PULSEG ( 3 ) 

REAL  0 

REAL  QFRACG ( 3 ) 

REAL  R 
REAL  T 

REAL  TIMUDRIV 
REAL  TIMUSTEP 
INTEGER* 4  TOSEED 
REAL  TSTEP 

S INCLUDE (  "/ INCLUDE /SSBLK0 3. DAT' ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


C 

C 

c 


MAIN  EXECUTION  LOOP 


C 

C 

c 

c 

c 

c 


c 

c 


Execution  of  all  events  is  performed 
within  this  loop 


onnonno  nnnnno  nonnono 
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c 


1000  CONTINUE 
♦LOOP*  START 


CALL  RECEIVE_REAL_32BIT (  P  ) 

CALL  RECEIVE_REAL_32BIT (  Q  ) 

CALL  R£CEIVE_REAL_32BIT(  R  ) 

CALL  RECEIVE_REAL_32BIT (  CIM(l)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(2)  ) 
CALL  RECEIVE_REAL_32BIT i  CIM(3)  ) 
CALL  RECEIVE_REAL_J^BIT (  CIM(4)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM<5)  ) 
CALL  RECEIVE_REAL_32BIT(  CIM<6)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM<7)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(8)  ) 
CALL  RECEIVE  REAL  32BIT<  CIM(9)  ) 


- C 

INERTIAL  MEASUREMENT  UPDATE  - C 

- C 

Get  inertial  measurement  data  needed  C 

for  guidance  calculations  .  C 

C 

- C 


IF  (  TSTEP  .GE.  TIMUDRIV  )  THEN 

TIMUDRIV  =  TIMUDRIV  +  TIMUSTEP 


- C 

GYRO  MODULE  - C 

- C 

Determine  sensed  body  rates  .  C 

C 

- C 


CALL  GYRO (T, P, Q, R, CIM, GYSEED, QFRACG, PULSEG) 
ENDIF 

CALL  SEND_REAL_32BIT(  PULSEG(l)  ) 

CALL  SEND_REAL_32BIT(  PULSEG12)  ) 

CALL  SEND  REAL  32BIT (  PULSEGP)  ) 


- C 

TERMINATION  LOGIC - C 

- C 

Defines  the  simulation  termination  C 

conditions  C 

C 

- C 


C  increment  time 

TSTEP  =  TSTEP  +  1.0 
T  -  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OP  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED_1 6BIT (  IEXIT  ) 

♦LOOP'  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
♦LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 1 9g/ debug /uubl X04 .for 


PROGRAM  BLK04 
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IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 


DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  CEI (9) 
DOUBLE  PRECISION 
REAL  CER  ( 9) 
DOUBLE  PRECISION 
REAL  CIM  (9) 

REAL  CIT (9) 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
INTEGER  IEXIT 
INTEGER  IRESLV 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  LAMSEK (2) 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  MAGRTR 
DOUBLE  PRECISION 
INTEGER  NSUB 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  Q 

DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  R 

DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  RRELTR ( 3 ) 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  TGOTR 
INTEGER* 4  TOSEED 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  •’RECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  VRELTR (3) 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 


AZSUB  C  00) 
CAZ (100) 

CEL  (100) 

CIE (9) 


CMS (9) 

CSK1 
CSK2 
CTI (9) 

DELT 

DTR 

ELSUB (100) 
GRT ( 5 ,  3) 


LAMDSK (2) 
LAMDTR (2) 
LAMDXX (2) 

LAMTRU (2) 

LATLP 

LATT 

LONGLP 

LONGT 

MAGLOS 

MGRDTR 


PI 

PTARG 

PTRGIC 

QTARG 

QTRC1C 

RJ<5) 

RJSUB(IOO) 
RRELM ( 3 ) 

RTAR ( 3 ) 

RTARG 

RTER (3) 

RTIC  (5,  3) 

RTRGIC 

SKOFF1 

SKOFF2 

SSK1 

SSK2 

T 


TPHI 

TPHID 

TPHI IC 

TPSI 

TPSID 

TPSI IC 

TRSUDRIV 

TRSUSTEP 

TSTEP 

TTHT 

TTHTD 

TTHTIC 

TTSUDRI V 

TTSUSTEP 

VRELM (3) 

VTAR (3) 
VTICI5,  3) 
X 

XD 

Y 

YD 

Z 


noon 
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DOUBLE  PRECISION  ZD 

$  INCLUDE ( • V INCLUDE /SSBLKO 4 . DAT  1 ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


c - C 

c -  MISSILE  STATE  INITIALIZATION  MODULE  - C 

c - c 

C  Initialize  integrated  missile  states  C 

C  C 

c - c 

C  COORDINATE  TRANSFORMATION  MATRICES 


C«LL  MMK (SNGL ( -90 . 0*DTR) , 1, SNGL (LATLP*DTR) , 2, 
SNGL (LONGLP*DTR) , 3,CEI) 


CIE(l) 

= 

CEI (1) 

C IE  (2) 

* 

CEI  (4) 

CIE (3) 

a 

CEI (7) 

CIE  (4) 

* 

CEI (2) 

CIE (5) 

* 

CEI (5) 

CIE  (6) 

= 

CEI (8) 

CIE (7) 

= 

CEI  (3) 

CIE (8) 

a 

CEI (6) 

CIE  (9) 

CEI  (9) 

MISSILE  TO  SEEKER  MATRIX  (  INCLUDES  MISALIGNMENT  ) 

SEEKER  MISALIGNMENT  DIRECTIONS  : 

SKOFF1  -  CONE  ANGLE  OFF  NORMAL  (  CURRENTLY  UNDEFINED  ) 
SKOFF2  «  POLAR  ANGLE 


C  NOTE:  TRANSFORMATION  INCLUDES  180  DEGREE  ROTATION  ABOUT  Y-AXIS 

SKOFF1  =0.0 

SKOFF2  =  2. 0*PI*RAN0 (TOSEED) 


CSK1 
SSK1 
CSK2 
SSK2 
CMS (1) 
CMS  (2) 
CMS  (3) 
CMS (4) 
CMS (5) 
CMS (6) 
CMS (7) 
CMS ( 8 ) 
CMS  (9) 


OCOS (SKOFF1 ) 
DSIN (SKOFF1) • 
DCOS (SKOFF2) 
DSIN (SKOFF2) 
-CSK1 
SSK1*CSK2 
SSK1*SSK2 
SSK1 *SSK2 
CSK1 

SSK1*CSK2 
SSK1 *CSK2 
SSK1 *SSK2 
-CSK1 


C  INITIALIZE  TARGET  BODY  RATES  (RAD/SEC) 

PTARG  =  PTRGIC*  DTR 
QTARG  =  QTRGIC'DTR 
RTARG  =  RTRGIC'DTR 

INITIALIZE  TARGET  EULER  ANGLES  (RAD) 

TPHI  =  TPHI IC*DTR 
TTHT  =  TTHTIC'DTR 
TPSI  =  TPSIIC*DTR 


C 

c 

c 

c 

c 

c 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  ail  events  is  performed  C 
within  this  loop  C 

C 


oonnnnnn 
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c 


c 


1000  CONTINUE 
♦LOOP*  START 


CALL  RECEIVE_REAL_648IT (  XD  ) 

CALL  RECEIVE_REAL_64BIT (  YD  ) 

CALL  RECEIVE_REAL_64BIT (  ZD  ) 

CALL  RECEIVE_REAL_64BIT (  X  ) 

CALL  RECEIVE_REAL_64BIT (  Y  ) 

CALL  R£CEIVE_REAL_648IT(  2  ) 

CALL  RECEIVE_REAL_32BIT (  Q  ) 

CALL  RECEIVE_REAL_32BIT (  R  ) 

CALL  RECEIVE_REAL_32BIT (  CIM(l)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM<2)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM<3>  ! 
CALL  RECEIVE_REAL_32BIT (  CIM<4)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(S)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(6)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(7)  ) 
CALL  RECEIVE_REAL_32BIT(  CIM(8)  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(9)  > 
CALL  RECEIVE_REAL_32BIT (  CER(l)  ) 
CALL  RECEIVE_REAL_32BIT (  CER(2)  ) 
CALL  RECEIVE_R£AL_32BIT (  CER(3)  ) 
CALL  RECEIVE_REAL_32BIT (  CER(4)  ) 
CALL  RECEIVE_REAL_32BIT (  CER(5)  ) 
CALL  RECEIVE_REAL_32BIT (  CER(6)  ) 
CALL  RECEIVE_REAL_32BIT (  CER<7)  ) 
CALL  RECEIVE_REAL_32BIT (  CER(8)  ) 
CALL  RECEIVE_REAL_32BIT (  CER(9)  ) 


TARGET  STATES  MODULE 


This  module  calculates  the  true  exo- 
atmospheric  trajectory  data  for 
the  target 


IF  (  TSTEP  .GE.  TTSUDRIV  )  THEN 
TTSUDRIV  =  TTSUDRIV  +  TTSUSTEP 

CALL  TARGET (  T, MAGRTR, CAZ, CEL, CER, CIE, PTARG, QTARC, RTARG, 

TPHI, TTHT, TPSI, GRT, TPHID, TTHTD, TPSID, CIT, RTIC, VTIC, 
RTAR, RTER.NSUB, IRESLV,RJ, CTI, VTAR, LATT, LONGT, 
AZSUB, ELSUB, R JSUB  ) 


ENDIF 


CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT < 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND_REAL_64BIT 
CALL  SEND  REAL  64BIT 


GRT  (1,1)  ) 

GRT  (1,2)  ) 
GRT  (1,3)  ) 
(  GRT  (2,1) 

(  GRT (2, 2) 


GRT (2, 3) 
GRT (3, 1) 
GRT (3, 2) 
GRT (3, 3) 
GRT (4, 1) 
GRT (4, 2) 
GRT (4, 3) 
GRT  (5, 1) 
GRT (5, 2) 
GRT (5, 3) 
VTIC (1 , 1) 
VTIC (1, 2) 
VTIC (1, 3) 
VTIC (2, 1) 
VTIC (2, 2) 
VTIC (2, 3) 
VTIC (3,1) 
VTIC ( 3 , 2 ) 
VTIC (3, 3) 
VTIC (4, 1) 
VTIC (4, 2) 


) 

) 

) 

) 

) 

) 

) 

) 


onooooon 
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CALL  SEND_REAL_64BIT (  VTIC(4,3) 
CALL  SEND_REAL_64BIT (  VTIC(5,1> 
CALL  SEND_R£AL_64BIT (  VriC(5,2) 
CALL  SEND_REAL_64BIT (  VTIC(5,3) 
CALL  SEND_REAL_64BIT (  RTIC(1,1) 
CALL  SEND_REAL_64BIT (  RTIC(1,2> 
CALL  SEND_REAL_64BIT (  RTIC(1,3) 
CALL  SENO_REAL_64BIT <  RTIC{2.1) 
CALL  SEND_REAL_64BIT (  RTIC(2,2) 
CALL  SEND_REAL_648IT (  RTIC(2,3) 
CALL  SEND_REAL_64BIT (  RTIC(3,1) 
CALL  SEND_REAL_64BIT (  RTIC(3,2) 
CALL  SEND_REAL_64BIT (  RTIC(3,3) 
CALL  SEND_REAL_64BIT (  RTIC(4,1) 
CALL  SEND_REAL_64BIT (  RTIC(4.2) 
CALL  SEND_REAL_64BIT (  RTIC<4,3) 
CALL  SEND_REAL_64BIT (  RTIC(5,1) 
CALL  SEND_REAL_64BIT (  RTIC(5,2) 
CALL  SEND_REAL  64BIT (  RTIC{5,3) 


) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 


C 

C 

c 

c 

c 

c 

c 


- c 

RELATIVE  STATES  MODULE  - C 

- c 

Calculate  relative  range,  range  rate,  C 

time-to-go,  LOS  angles  and  rates  C 

C 

- c 


IF  (  TSTEP  .GE.  TRSUDRIV)  THEN 
TRSUDRIV  =  TRSUDRIV  +  TRSUSTEP 

CALL  RELAT (RTIC, VTIC, X,  Y ,  Z ,  XD,  YD,  ZD,  Q,  R,  CIM, CMS, RRELTR, 
MAGRTR, VRELTR, MGRDTR, MAGLOS, LAMTRU, LAMDXX, 
LAMDTR, LAMSEK, LAMDSK, TGOTR,  RRELM, VRELM,  CAZ, CEL) 


ENDIF 

CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL_64BIT ( 
CALL  SEND_REAL  32BIT ( 
CALL  SEND  REAL  328IT< 


MAGRTR  ) 
LAMDXX (1) 
LAMDXX (2) 
LAMSEK (1) 
LAMSEK (2) 


) 

) 

) 

) 


CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 


SEND_REAL_32BIT< 
SEND_REAL~32BIT< 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL  32BIT ( 
SEND_REAL~32BIT ( 
SEND  REAL  32BIT( 


RRELTR (1)  ) 
RRELTR (2)  ) 
RRELTR (3)  ) 
VRELTR (1)  ) 
VRELTR (2)  ) 
VRELTR (3)  ) 
TGOTR  ) 


c - - - - - C 

C -  TERMINATION  LOGIC  - C 

C - C 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

c - c 

C  increment  time 

TSTEP  «  TSTEP  +  1 . 0D0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  RECEIVE_SIGNED_1 6BI T {  IEXIT  ) 
•LOOP*  STOP 

I  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 1 9g/debug/uublX05 .for 
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PROGRAM  BLK0  5 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  CG (3) 

REAL  CGEST ( 3 ) 

REAL  CMMD (2) 

REAL  CNALP 
REAL  DELT 
REAL  DLPC 
REAL  DLYC 
REAL  DTEPS 
REAL  DTFRU 
REAL  DTMP1 
REAL  EPSL 
INTEGER  IBAUTO 
INTEGER  IEXIT 
INTEGER  IFTAB 
REAL  IYY 
REAL  IZZ 
REAL  KME 
REAL  KNE 
REAL  KTHT 
REAL  KTHTD 
REAL  LFRACS 
REAL  MALPHA 
REAL  MASS_ 

REAL  MDELTA 
REAL  PSIER 
REAL  RMIR_ ( 3 ) 

REAL  SQ 
REAL  SR 
REAL  T 

REAL  TAPUDRIV 
REAL  TAPUSTEP 
REAL  TFRAC 
REAL  TFRCS 
REAL  TFTA9 
REAL  THTER 
REAL  TI2M ( 9) 

REAL  TSTEP 
REAL  TSTG1 
REAL  TSTG2 
REAL  VCMD14) 

INTEGER  VLVCM5 
REAL  VMIR_ ( 3 ) 

REAL  XCPCG 
REAL  XDEL 

SINCLUDE  ( '  '“/INCLUDE /SSBLKC 5  .  DAT  •  ) 

'LOOP*  PROLOGUE 

'  INITIALIZE  80x87 
CALL  CW87 


C 

C 

C 

C 

C 

c 

c 


1000  CONTINUE 
•LOOP'  START 

CALL  RECEIVE_REAL_32BIT (  MASS_  ) 
CALL  RECEIVE_REAL_32BIT (  CG ( 1 )  ) 
CALL  RECEIVE_REAL_32BIT (  CG(2)  ) 
CALL  RECEIVE_REAL_32BIT (  CG(3)  ) 
CALL  RECEIVE_REAL_32BIT (  IYY  ) 

CALL  RECEIVE_REAL_323IT (  IZZ  ) 

CALL  RECEIVE_REAL_323IT (  RMIR_(1)  ) 
CALL  RECEIVE_REAL_32BIT (  RMIR_(2)  ) 
CALL  RECEIVE_REAL_32BIT (  RMIR_(3)  ) 
CALL  RECEIVE  REAL  32BIT (  VMIR  (1!  ) 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 

within  this  loop  C 

C 

- c 


o  o  o  o  o  n 
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CALL  RECEIVE_REAL_328I T ( 
CALL  RECEIVc,_REAL_323IT  ( 
CALL  RECEIVE_REAL_328IT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_kEAL_32BIT( 
CALL  PECEIVE_REAL_32BIT ( 
CALL  RF.CEIVF_REAL_32BIT  ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BI T ( 
CALL  RECEIVE_REAL_32BIT ( 
CALI  RECEIVE  REAL_32BIT( 
CALu  RECEIVE~REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL  32BIT< 


VMIR_(2)  ) 
VMIR_(3)  i 
SQ  ) 

SR  ) 

TI2MC)  ) 
TI2M (2)  ) 
TI 2M  ( 3 )  ) 
TI2MM)  ) 
TI2MI5)  ) 
TI2M(6)  ) 
TI2M  (7)  ) 
TI2M(8)  ) 

TI2MI9)  ) 
THTER  ) 
PSIER  ) 


C - 

C -  AUTOPILOTS 

C - 

C 

C - 


IF  (  TSTEP  .  GE.  TAPUDRIV  )  THEN 
*  TAPUDRIV  =  TAPUDRIV  +  TAPUSTE? 

IF  (  T.LT.TSTG2  )  THEN 

C  CGEST  TEMPORARILY  EQUAL  TO  CG 

CGEST(l)  =  CG ( 1 ) 

CGEST  1 2 )  =  CG ( 2 ) 

CGEST (3)  =  CG ( 3 ) 


BOOST  AUTOPILOT  MODULE 


Computes  commands  to  the  steering  devices 


C  FIRST  STAGE  SEPARATION 

IF  (  ABS (T-TSTG1) .LE.DTEPS  )  THEN 
IBAUTO  =  1 
ENDIF 

CALL  BAUTO(T, THTER, PSIER, SQ, SR, MASS_, IYY, IZ3, CGEST, TI2M, 
RMIR_, VMIR_, IBAUTO, CMMD, DLPC, DLYC, KTHT, KTHTD, XDEL, XCPCG, 
LFRACS, CNALP , MDELTA, KNE, KME, MALPHA) 

ENDIF 
ENDIF 
*  bauto 

CALL  SEND_REAL_32BIT (  CMMD ( 1 )  ) 

CALL  SEND  REAL  32BIT(  CMMD (2)  ) 


C 

C 

C 


C 


FRACS  LOGIC  MODULE  - C 

- - c 

Models  FRACS  hysteresis  logic  C 

C 

- c 


IF  t  TSTEP  .GE.  TAPUDRI'  )  THEN 
TAPUDRIV  =  TAPUDRIV  +  TAPUSTEP 
IF  (  T.LT.TSTG2  )  THEN 

IF  (  T.GF..TFRCS  .AND.  T .  GE .  TFRAC  )  THEN 
CALL  FRACS (T, DLPC, DLYC, VCMD , VLVCMS ) 

SET  FLAG  TO  COMPUTE  FRACS  THRUSTER  RESPONSE  TABLE 


oonooo  noono 
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IFTAB  =  1 
TFTAB  =  T 


C  SCHEDULE  NEXT  FRACS  CALCULATION 

DTMP1  =  DTFRU  *  ANINT  (  (Ti-DTFRU)  /  DTFRU  ) 
TFRAC  =  DTMP1  -  EPSL 


ENDIF 

ENDIF 


ENDIF 
*  fracs 

CALL  SEND_REAL_32BIT (  VCMD(l)  ) 
CALL  SEND_REAL_32BIT (  VCMD (2)  ) 
CALL  SEND_REAL_32BIT (  VCMD (3)  ) 
CALL  SEND_REAL_32BIT (  VCMD ( 4 )  ) 
CALL  SEND_SIGNED_1 6BIT (  IFTAB  ) 
CALL  SEND  REAL  32BIT (  TFTAB  ) 


*  The  IFTAB  assignment  was  moved  from  the  partition  with  FRCTHR 
IFTAB  =  0 


c - C 

c -  TERMINATION  LOGIC - C 

c - c 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

c - c 

C  increment  time 

TSTEP  =  TSTEP  +  1 . 000 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  rfrCEIVE__SIGNED_l  6BIT  {  IEXIT  ) 
•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 1 9g/debug/uublX06 .for 


PROGRAM  BLK06 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  ALT 
REAL  CEI (9) 

REAL  CER (9) 

REAL  CIE  (9) 

REAL  CIM ( 9 ) 

REAL  CIR ( 9 ) 

REAL  CRI (9) 

REAL  DELT 
REAL  DTR 
INTEGER  IEXIT 
REAL  LAT 
REAL  LATLP 
REAL  LONG 
REAL  LONGLP 
REAL  MVRWM 
REAL  OMEGAE 
REAL  RADE 
REAL  SHEAR 
REAL  T 
REAL  TSTEP 
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REAL  TSTG2 
REAL  VRWM ( 3 ) 

REAL  VWIND 
REAL  XD_ 

REAL  XYZE_ { 3) 

REAL  XYZH (3) 

REAL  X_ 

REAL  YD_ 

REAL  Y_ 

REAL  ZD 
REAL  Z_ 

$INCLUDE ( ' "/ INCLUDE /SSBLKO  6 . DAT  1 ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 


c - c 

C - MISSILE  STATE  INITIALIZATION  MODULE - C 

c - c 

C  Initialize  integrated  missile  states  C 

C  C 

c - c 


C  COORDINATE  TRANSFORMATION  MATRICES 

CALL  MMK (~90.0*DTR, 1, LATLP*DTR, 2, LONGLP*DTR, 3,CEI) 


CIE(l) 

= 

CEI  (1) 

CIE (2) 

= 

CEI (4) 

CIE  (3) 

= 

CEI (7) 

CIE<4) 

*= 

CEI (2) 

CIE ( 5 ) 

* 

CEI (5) 

CIE  ( 6) 

* 

CEI (8) 

CIE (7) 

- 

CEI  (3) 

CIE  (8) 

= 

CEI  (6) 

CIE  ( 9) 

* 

CEI (9) 

C 

C 

C 

C 

c 

c 

c 


- - c 

MAIN  EXECUTION  LOOP  - C 

- C 

Execution  of  all  events  is  performed  C 

within  this  loop  C 

C 

- C 


1000  CONTINUE 
•LOOP*  START 


CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL_32BIT( 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE~REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT  < 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL  32BIT( 
CALr  RECEIVE_REAL~32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL  32BIT ( 


XD_  ) 

YD_  ) 

ZD_  ) 

x_  ) 

Y_  > 

Z_  > 

CIM(l)  ) 
CIM ( 2 )  ) 
CIM (31  ) 

CIM(4)  ) 
CIM  {  5 )  ) 
CIM { 6)  ) 
CIM  <  7 )  ) 
CIM  ( 8  )  ' 
CIM  1 9)  ) 
XYZE_ ( 1 )  ) 
XYZE_ ( 2 )  ) 
XYZE  (3)  ) 


C  ROTATING  EARTH  MODEL 


CALL  MMK (0. 0, 1,0.0, 2, OMEGAE'T, 3,CER> 

*  CER  used  to  be  recalculated  later,  along  with  other  values 

*  associated  with  the  rotating  earth  model.  We  now  use  only 


oooonoo  onoono 
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*  these  values  derived  from 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND  REAL32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND  REAL  32BIT ( 


the  first-order  estimates 
CER(l)  ) 

CER  ( 2 )  ) 

CER  ( 3  )  ) 

CER  (4)  ) 

CER ( 5 )  ) 

Ct-Klo)  ) 

CER  (7)  ) 

CER (8)  ) 

CER (9)  ) 


XYZR(l) 
XYZR (2) 
XYZR (3) 


CER ( 1 ) *XYZE  (1) 
CER  (2)  *XYZE~(1) 
CER  (  3)  *XYZE  (1) 


CEP ( 4 ) *XYZE_(2) 
CER (5) *XYZE_(2) 
CER ( 6) *XYZE  (2) 


+  CER ( 7 ) *XYZE_ ( 3 ) 
+  CER ( 8 ) *XYZE_ (3) 
+  CER  ( 9) *XVZE_ (3 ) 


CIR ( 1 ) 
CIR (2) 
CIR ( 3 ) 
CIR ( 4 ) 
CIR ( 5 ) 
CIR  (6) 
CIR  (7) 
CIR (8) 
CIR  (9) 


CER (1 )  *CIE  (1 ) 
CER  (2)  *CIE<1) 
CER ( 3 ) *CIE(1) 
CER (1) *CIE  (4) 
CER (2) *CIE  (4) 
CER(3)*CIE<4) 
CER (1 ) *CIE ( 7 ) 
CER (2) *CIE (7) 
CER (3) *CIE (7) 


+  CER (4) *CIE (2) 
+  CER  (5)  'CIE  (2) 
+  CER  (  6)  *CIE<2) 
+  CER ( 4 ) *CI£<5) 
+  CER(5)*CIE(5) 
+  CER (6) *CIE (5) 
+  CER  ( 4 )  *CIE(8) 
+  CER ( 5 ) *CIE (8) 
+  CER (6) *CIE (8) 


+  CER (7) *CIE  (3) 
+  CER (8) *CIE (3) 
+  CER  (9)  *CIE  (3) 
+  CER  (7)  *CIE  (6) 
+  CER (8) *CIE (6) 
+  CER  (  9)  *CIE  (  6) 
+  CER(7)*CIE(9) 
+  CER (8) *CIE (9) 
+  CER  (9)  *CIE  (9) 


CRI (1)  =  CIR ( 1 ) 

CRI (2)  =  CIR { 4 ) 

CRI  (3)  =  CIR (7) 

CRI (4)  =  CIR (2) 

CRI (5)  =  CIR  (5) 

CRI (6)  =  CIR (8) 

CRI  (7)  =  CIR (3) 

CRI (8)  =  CIR (6' 

CRI  (9)  =  CIR  (  9) 


C  CALCULATE  CURRENT  LATITUDE  AND  LONGITUDE 


LAT  =  ATAN2  (XYZR(3)  ,  SQRT  (XYZR  (1)  "2  +  XYZR  (2)  **2)  ) /DTR 
LONG  =  ATAN2 (XYZR (2 ) , XYZR ( 1 ) ) /DTR 


C  CALCULATE  CURRENT  MISSILE  ALTITUDE 


ALT  =  SQRT  (  X_**2  +  Y_**2  +  Z_**2  )  -  RADE 
CALL  SEND  REAL  32BIT (  ALT  ) 


ATMOSPHERE  MODULE  - 

Computes  the  atmospheric  properties 


C 

■C 

c 

c 

c 

■c 


IF  (  T.LT.TSTG2  )  THEN 

CALL  ATMOS2 (T, ALT, XD  , YD_, ZD_, CIM, CRI r LAT, LONG, 
VWIND, SHEAR, VRWM, MVRWM) 

ENDIF 

CALL  SEND_REAL_32BTT (  VRWM<1!  ) 

CALL  SEND_REAL_32BIT (  VRWM (2)  ) 

CALL  SEND_REAL_32BIT (  VRWM (3)  ) 

CALL  SEND  REAL  32BIT (  MVRWM  ) 


- C 

TERMINATION  LOGIC  - C 

- C 

Defines  the  simulation  termination  C 

conditions  C 

C 

- C 


C  increment  time 

TSTEP  =  TSTEP  *1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MFT 
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CALL  RECEIVE_SIGNED_16BIT (  IEXIT  ) 
•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
*  LOOP  *  EPILOGUE 
END 


FILE:  uuv22 . 19g/debug/uublk07 .for 


PROGRAM  BLK07 

IMPLICIT  REAL 
IMPLICIT  REAL 

REAL  CG (3) 
REAL  CMMD ( 2 ) 
REAL  DELT 
REAL  DLP 
REAL  DLPD 
REAL  DLPIC 
REAL  DLY 
REAL  DLYD 
REAL  DLYIC 
REAL  DTEPS 


REAL 

DTOFFVM) 

REAL 

EISP 

REAL 

FOFF1 (4) 

REAL 

FOFF2 (4) 

REAL 

FXT 

REAL 

FXVCS 

REAL 

FYT 

REAL 

FYVCS 

REAL 

FZT 

REAL 

FZVCS 

INTEGER  IBTHR 

INTEGER  IEXIT 

INTEGER  IVCS 

INTEGER  IVTAB 

REAL 

MDOTT 

REAL 

MDOTV 

REAL 

MXT 

REAL 

MXVCS 

REAL 

MYT 

REAL 

MYVCS 

REAL 

MZT 

REAL 

MZVCS 

REAL 

PMAX 

REAL 

PRESS 

REAL 

T 

REAL 

TBRK 

REAL 

TBURNM 

REAL 

THR 

REAL 

THRV 

REAL 

TIMONV 

REAL 

TINHIB 

REAL 

TKVON 

REAL 

TOFFLT (4) 

INTEGER* 4  TOSEED 

REAL 

TOTDEL 

REAL 

TSTEP 

REAL 

TSTG1 

REAL 

TSTG2 

REAL 

TVTAB 

SINCLUDE ( *  VINCLUDE/SSBLK07 . DAT ' ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 

CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 

*  initialization  for  purpose  of  delaying  receipt  of  actual  values 

PRESS  =  2116.25 


(A-H) 

(O-Z) 


o  o  o  o  o  o  *  oooonon  o  o  o  o  n  o  n 
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MISSILE  STATE  INITIALIZATION  MODULE  - 


Initialize  integrated  missile  states 


--C 
—  C 
--C 

c 

c 

— c 


INITIAL  TVC  NOZZLE  POSITION 

DLP  =  DLPIC 
DLY  =  DLYIC 
DLPD  =0.0 

DLYD  =0.0 

CALL  INTEGI  (  DLP  ,  DLPD  ,  T  ,  19  ) 

CALL  INTEGI  (  DLY  ,  DLYD  ,  T  ,  20  ) 


MAIN  EXECUTION  LOOP 


Execution  of  all  events  is  performed 
within  this  loop 


1000  CONTINUE 
LOOP*  START 

from  MASSPR 

CALL  RECEIVE_REAL_32BIT (  EISP  ) 
CALL  RECEIVE_REAL  32BIT (  CG<1)  ) 
CALL  RECEIVE_REAL~32BIT (  CG(2)  ) 
CALL  RECEIVE_REAL  32BIT (  CG<3)  ) 


BOOSTERS  MODULE 


IF  (  T.LE.TSTG2  )  THEN 

CALL  BTHRST (T,  CG, EISP, PRESS, DLP, DLY, TOSEED, TBRK, IBTHR, 
FXT, FYT, FZT, MXT, MYT, MZT, MDOTT, THRV, THR) 

ENDIF 

IF  (  ABS (T-TSTG1) .LE.DTEPS  )  THEN 
IBTHR  =  1 
ENDIF 

CALL  SEND_REAL_32BIT (  FXT  ) 

CALL  SEND_REAL_32BIT (  FYT  ) 

CALL  SEND_REAL_32BIT (  FZT  ) 

CALL  SEND_REAL_32BIT (  MXT  ) 

CALL  SEND_REAL_32BIT (  MYT  ) 

CALL  SEND_REAL_32BIT (  MZT  ) 

CALL  SEND_REAL  32BIT (  MDOTT  ) 


C 

C 

C 

c 

c 

c 

c 


- c 

NOZZLE  CONTROL  UNIT  MODULE  - C 

- c 

Models  the  response  of  the  nozzle  C 

control  unit  during  first  stage  C 

C 

- c 


IF  (  T.LE.TSTG1  .AND.  T.GT.TINHIB)  THEN 
CALL  NCU (DLP, DLY, CMMD, DLPD, DLYD) 
ENDIF 


*  from  frcthr 

CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVEREAL32BIT ( 
CALL  RECEIVE  REAL  32BITI 


FOFFl(l)  ) 
FOFF1 ( 2 )  ) 
FOFF1 (3)  ) 
FOFFl(l)  ) 
FOFF2 ( 1 )  ) 


nnnnn  oooonon 


oonoono 
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CALL  RECEIVE_REAL_32BIT (  FOFF2(2>  ) 
CALL  RECEIVE_REAL_32BIT (  FOFF2(3)  ) 
CALL  RECEIVE  REAL  32BIT(  FOFF2(4)  ) 


VCS  THRUSTER  RESPONSE  MODULE 


Determines  the  forces  and  moments 
imparted  by  the  VCS  thrusters 


IF  (  T.GE.TKVON  )  THEN 

CALL  VCSTH1 (T, CG, TBURNM, IVCS , TOFFLT, TIMONV, DTOFFV, 

TVTAB, FOFF1 , F0FF2, IVTAB, FXVCS, FYVCS, FZVCS, 

MXVCS, MYVCS, MZVCS, MDOTV) 

ENDIF 

*  from  ATMOS  (delayed) 

CALL  RECEIVE_REAL_32BIT (  PRESS  ) 

CALL  SEND_REAL_32BIT (  FXVCS  ) 

CALL  SEND_REAL_32BIT (  FYVCS  ) 

CALL  SEND_REAL_32BIT (  FZVCS  ) 

CALL  SEND_REAL_32BIT<  MXVCS  ) 

CALL  SEND_REAL_32BIT (  MYVCS  ) 

CALL  SEND_REAL_32BIT (  MZVCS  ) 

CALL  SEND_REAL_32BIT (  MDOTV  ) 

IF  (  T.LE.TSTG1  )  THEN 

CALL  INTEG  (  DLP  ,  DLPD  ,  T  ,  19  ) 

CALL  INTEG  (  DLY  ,  DLYD  ,  T  ,  20  ) 

TOTDEL  =  SQRT  (  DLP**2  +  DLY* *2  ) 

IF  (  TOTDEL. GT.PMAX  )  THEN 
DLP  =  DLP*PMAX/TOTDEL 

DLY  =  DLY *PMAX /TOTDEL 

ENDIF 
ENDIF 

C  FIRST  STAGE  SEPARATION 

IF  (  ABS(T-TSTGl) .LE.DTEPS  )  THEN 
DLP  =0.0 

DLY  =0.0 

ENDIF 

*  guidance 

CALL  RECEIVE_SIGNED_16BIT (  IVCS  ) 

*  bauto 

CALL  RECEIVE_REAL_328IT (  CMMD ! 1 )  ) 

CALL  RECEIVE_REAL_32BIT (  CMMD (2)  ) 

*  vcslog 

CALL  RECEIVE  REAL_32BIT (  DTOFFV (1)  ) 

CALL  RECEIVEjREAL_32BIT (  DTOFFV<2)  ) 

CALL  RECEIVE_REAL_32BIT (  DTOFFV(3)  ) 

CALL  RECEIVE_REAL_32BIT (  DTOFFV (4)  ) 

CALL  RECEIVE_SIGNED_1 6BIT (  IVTAB  ) 

CALL  RECEIVE_REAL_32BIT (  TBURNM  ) 

CALL  RECEIVE_REAL_32BIT (  TIMONV  ) 

CALL  RECEIVE_REAL_32BIT (  TOFFLT (1)  ) 

CALL  RECEIVE_REAL_32BIT (  TOFFLT (2)  ) 

CALL  RECEIVE_REAL_32BIT (  TOFFLT (3)  ) 

CALL  RECEIVE_REAL_32BIT (  TOFFLT (4)  ) 

CALL  RECEI VE_REAL_3  2 BIT (  TVTAB  ) 

- c 

TERMINATION  LOGIC - C 

- c 

Defines  the  simulation  termination  C 

conditions  C 

C 

- c 

C  increment  time 

TSTEP  =  TSTEP  +  1.0 
T  =  TSTEP  *  DELT 


C 

C 

■C 

C 

C 

c 

c 
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C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  R£CEIVE_SIGNED_1 6BIT (  IEXIT  ) 

•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE:  uuv22 . 1 9g/debug/uublk08 .for 


PROGRAM  BLK08 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  CIM ( 9) 

REAL  CMI (9) 

REAL  DELT 
REAL  DTR 
REAL  FRCX 
REAL  FXA 
REAL  FXACS 
REAL  FXT 
REAL  FXVCS 
INTEGER  IEXIT 
REAL  I XX 
REAL  IYY 
REAL  IZZ 
REAL  MASS_ 

REAL  MRCX 
REAL  MRCY 
REAL  MRCZ 
REAL  MX 
REAL  MXA 
REAL  MXACS 
REAL  MXT 
REAL  MXVCS 
REAL  MY 
REAL  MYA 
REAL  MYACS 
REAL  MYT 
REAL  MYVCS 
REAL  MZ 
REAL  MZA 
REAL  MZACS 
REAL  MZT 
REAL  MZVCS 
REAL  P 
REAL  PD 
REAL  PHI 
REAL  PHIICD 
REAL  PQR ( 3 ) 

REAL  PS I 
REAL  PSIICD 
REAL  Q 
REAL  QD 
REAL  QUATM) 

REAL  QUATDM) 

REAL  QUATICM) 

REAL  QUATM 
REAL  R 
REAL  RD 
REAL  T 
REAL  THT 
REAL  THTICD 
REAL  TMP I 
REAL  TSTEP 
REAL  X_ 

REAL  Y_ 

REAL  Z_ 

S INCLUDE ( ' "/ INCLUDE /SSBLK 08 . DAT ' ) 


LOOP*  PROLOGUE 
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*  INITIALIZE  80x87 

CALL  CW8  7 

*  initialization  of  variables  not  computed  until  end  of  blk.00 


PD 

«  0.0 

QD 

=  0.0 

RD 

=  0.0 

C - C 

c - MISSILE  STATE  INITIALIZATION  MODULE - C 

C - C 

C  Initialize  integrated  missile  states  C 

C  C 

c - C 

C  INITIAL  MISSILE  EULER  ANGLES  IN  RADIANS 


PHI  -  PHIICD*OTR 
THT  -  THTICD'DTR 
PSI  -  PSIICD'DTR 

C  COMPUTE  INERTIAL  TO  MISSILE  TRANSFORMATION  MATRIX 

CALL  MMK(PHI,1,THT,2,PSI,3,CIM) 

CMI  (1)  =  CIM(l) 

CMI (2)  =  CIM<4) 

CMI (3)  =  CIM (7) 

CMI (4)  -  CIM (2) 

CMI (5)  =  CIM ( 5 ) 

CMI (6)  =  CIM ( 8 ) 

CMI (7)  -  CIM (3) 

CMI (8)  »  CIM (6) 

CMI  (9)  =  CIM  (9) 

CALL  BXI2FV (QUATM, CMI , QUATIC) 

PQR<1)  =  P 
PQR (2)  «  Q 
PQR  (3)  -  R 

CALL  FVDOT (PQR, TMP1, QUATIC, QUATD) 

QUAT(l)  =  QUATIC  (1) 

QUAT (2)  =  QUATIC (2) 

QUAT ( 3 )  =  QUATIC (3) 

QUAT (4)  -  QUATIC (4) 

C  INITIALIZE  MISSILE  TRUTH  STATES 


CALL 

INTEGI 

P 

PD 

T  , 

12  ) 

CALL 

INTEGI 

Q 

QD 

T  , 

13  ) 

CALL 

INTEGI 

R 

RD 

T  , 

14  ) 

CALL 

INTEGI 

QUAT ( 1 ) 

QUATD (1) 

T  , 

15  ) 

CALL 

INTEGI 

QUAT (2) 

QUATD (2) 

T  , 

16  ) 

CALL 

INTEGI 

QUAT (3) 

QUATD  (3) 

T  , 

17  ) 

CALL 

INTEGI 

QUAT ( 4 ) 

QUATD ( 4 ) 

T  , 

18  ) 

C 

C 

C 

C 

C 

C 

C 


- c 

MAIN  EXECUTION  LOOP - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
•LOOP*  START 


- C 

- C 

- c 

Integrate  missile  states  to  current  time  C 

C 

- c 


MISSILE  STATE  UPDATE  MODULE 


*  tmsudriv  is  no  longer  needed  --  IF/ENDIF  and  assignment  deleted 


noon  nnnooo 
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The  extrapolated  states  have  been  deleted.  There  should  be  no  need 
to  look  into  the  future. 

Note  that  the  states  which  follow  have  all  been  initialized,  and  each 
is  integrated  at  the  end  of  the  timestep. 

CALL  REC£IVE_REAL_32BIT (  X_  ) 

CALL  R£CEIVE_REAL_32BIT (  Y_  ) 

CALL  RECEIVE_REAL_32BIT (  Z_  ) 

CALL  SEND_REAL_32BIT(  P  ) 

CALL  SEND  REAL_32BIT (  Q  ) 

CALL  SEND~REAL_32BIT (  R  ) 

CALL  SEND_REAL_32BIT (  QUAT  (1 )  ) 

CALL  SEND_REAL_32BIT (  QUAT (2)  ) 

CALL  SEND_REAL_32BIT (  QUAT (3)  ) 

CALL  SEND  REAL  32BIT (  QUAT (4)  ) 


CALL  RECEIVE_REAL_32BIT (  MASS_  ) 

CALL  RECEIVE_REAL_32BIT (  CIM(l)  ) 

CALL  RECEIVE_REAL_32BIT (  CIM(2)  ) 

CALL  RECEIVE_REAL_32BIT (  CIM<3)  ) 

CALL  RECEIVE_REAL_32BIT <  CIM<4)  ) 

CALL  RECEIVE_REAL_32BIT (  CIM<5)  ) 

CALL  RECEIVE_REAL_32BIT(  CIM<6)  ) 

CALL  RECEIVE_REAL_32BIT (  CIM(7)  ) 

CALL  RECEIVE_REAL_32BIT <  CIM(8)  ) 

CALL  RECEIVE_REAL_32BIT (  CIM<9>  ) 
initialization  of  these  variables  was  added 


CALL  SEND_REAL_32BIT<  PD  ) 
CALL  SEND_REAL_32BIT (  QD  ) 
CALL  SEND  REAL  32BIT (  RD  ) 


CALL  RECEIVE_REAL_32BIT (  IXX  ) 
CALL  RECEIVE_REAL_32BIT (  IYY  ) 
CALL  RECEIVE  REAL  32BIT (  IZZ  ) 


C 


C 


C 


C 


C 


from  BTHRST 

CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  REC£IVE_REAL_32BIT< 
CALL  RECEIVE_REAL_32BIT ( 
from  FRCTHR 

CALL  RECEIVE  REAL  32BIT< 
CALL  RECEIVE~REAL~32BIT( 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT( 
from  AERO 

CALL  RECEIVE_REAL_32BIT< 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT( 
CALL  RECE I VE_REAL_3 2 B I T ( 
from  ACSTHR 

CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
from  VC ST HR 

CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL  32BIT ( 


EXT  ) 
MXT  ) 
MYT  ) 
MZT  ) 

FRCX  ) 
MRCX  ) 
MRCY  ) 
MRCZ  ) 

FXA  ) 
MXA  ) 
MYA  } 
MZA  ) 

FXACS  ) 
MXACS  ) 
MYACS  ) 
MZACS  ) 

FXVCS  ) 
MXVCS  ) 
MYVCS  ) 
MZVCS  ) 


VEHICLE  STATES  MODULE  - 

Compute  missile  state  derivatives 


CALL  MISSLR (T, QUAT, CIM, P, Q, R, IXX, IYY, IZZ, MASS_, FXA, FXT, 
FRCX, FXACS, FXVCS, 

MXA, MXT, MRCX, MXACS, MXVCS, 

MYA, MYT, MRCY, MYACS, MYVCS, MZA, MZT, MRCZ, MZACS, 
MZVCS,  X_,  Y_,  Z_,  PD,  QD,  RD, 

MX, MY, MZ, 

QUATD) 


MISSILE  STATE  INTEGRATION  MODULE 


Revise  missile  states  using  derivatives 


noon  nnonoo 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


just  computed  .  Missile  states  must  not  C 
be  integrated  if  a  table  lookup  index  C 
transition  has  occurred  since  the  last  C 
integration  step  .  The  next  integration  C 
step  should  be  rescheduled  to  coincide  C 
with  the  earliest  detected  table  lookup  C 
index  transition  instead  .  Otherwise  C 

schedule  the  next  integration  step  to  C 
occur  at  the  default  step  size  .  C 

C 

- C 


C  TRAPEZOIDAL  INTEGRATION  FOR  SIMPLICITY 


CALL 

INTEG 

(  P 

PD 

,  T 

12 

) 

CALL 

INTEG 

(  Q 

QD 

,  T  ,  13 

) 

CALL 

INTEG 

(  R 

RD 

,  T  ,  14 

) 

CALL 

INTEG 

(  QUAT(l) 

QUATD  (1) 

,  T 

15 

) 

CALL 

INTEG 

(  QUAT (2 ) 

QUATD (2) 

,  T  ,  16 

) 

CALL 

INTEG 

(  QUAT (3) 

QUATD (3) 

,  T  ,  17 

) 

CALL 

INTEG 

(  QUAT  (4) 

QUATD (4) 

,  T  ,  18 

) 

TERMINATION  LOGIC 


Defines  the  simulation  termination 
conditions 


C  increment  time 

TSTEP  -  TSTEP  +1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED_16BIT(  IEXIT  ) 

•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 1 9g/debug/uublk09 . for 


PROGRAM  BLK09 

IMPLICIT  REAL 

IMPLICIT  REAL 

REAL 

ACS LEV 

REAL 

AFX ACS 

REAL 

AFYACS 

REAL 

AFZACS 

REAL 

AMDOTA 

REAL 

AMXACS 

REAL 

AMYACS 

REAL 

AMZACS 

REAL 

BFXACS 

REAL 

BFYACS 

REAL 

BFZACS 

REAL 

BMDOTA 

REAL 

BMXACS 

REAL 

BMYACS 

REAL 

BMZACS 

REAL 

CG<3) 

REAL 

DELT 

REAL 

DTACSA ( 4 ) 

REAL 

FXACS 

REAL 

FYACS 

REAL 

FZACS 

INTEGER  IACSONA 

o  n  n  n  o  o  o 


nnoonoo  n  *  ooonono 
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INTEGER  IEXIT 
INTEGER  ITHRES 
REAL  MDOTA 
REAL  MXACS 
REAL  MYACS 
REAL  MZACS 
REAL  T 
REAL  TATAB 
REAL  TKVON 
INTEGER* 4  TOSEEO 
REAL  TSTEP 

$INCLUDE ( ' "/INCLUDE/SSBLKO  9 . DAT  1 ) 

♦LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 

within  this  loop  C 

C 

- c 


1000  CONTINUE 
LOOP*  START 


from  MASSPR 

CALL  RECEIVE_REAL_32BIT (  CG<1)  ) 
CALL  RECEIVE_REAL_32BIT (  CG<2)  ) 
CALL  R£CEIVE_REAL  32BIT (  CG<3)  ) 


ACS  THRUSTER  RESPONSE  MODULE 


Determines  the  forces  and  moments 
imparted  by  the  ACS  thrusters 


IF  (  T.GE. TKVON  )  THEN 

CALL  ACSTHA (T, CG, ACSLEV, DTACSA, TATAB, TOSEED, 

ITHRES, AFXACS , AF YACS , AFZ ACS , AMXACS , AMYACS , AMZ AC  S , 
AMDOTA, IACSONA) 

ENDIF 

CALL  RECEIVE_REAL_32BIT (  BFXACS  ) 

CALL  RECEIVE_REAL_32BIT (  BFYACS  ) 

CALL  RECEIVE_REAL_32BIT <  BFZACS  ) 

CALL  RECEIVE_REAL_32BIT (  BMXACS  ) 

CALL  RECEIVE_REAL_32BIT (  BMYACS  ) 

CALL  RECEIVE_REAL_32BIT  (  BM.’ACS  ) 

CALL  RECEIVE_REAL_32BIT(  BMDOTA  ) 

IF  (  T.GE. TKVON  )  THEN 

FXACS  =  BFXACS  +  AFXACS 
FYACS  =  BFYACS  +  AFYACS 
FZACS  =  BFZACS  +  AFZACS 
MXACS  *  BMXACS  +  AMXACS 
MYACS  =  BMYACS  +  AMYACS 
MZACS  =  BMZACS  +  AMZACS 
MDOTA  =  BMDOTA  +  AMDOTA 

ENDIF 

CALL  SEND_SIGNED_16BI T<  IACSONA  ) 

CALL  SEND_REAL_32BIT (  FXACS  ) 

CALL  SEND_REAL_32BIT (  FYACS  ) 

CALL  SEND_REAL_32BIT (  FZACS  ) 

CALL  SEND  REAL  32BIT (  MXACS  ) 


n  o  o  o  o  o  n 
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-ALL  SFND_REAL_32BIT (  MYACS  ) 

CALL  SEND_REAL_32BIT (  MZACS  ) 

CALL  SEND_REAL_32BIT (  MDOTA  ) 

*  kvauto 

CALL  RECEIVE_REAL_32BIT (  ACSLEV  ) 
CALL  RECEIVE_SIGNED_16BIT {  ITHRES  ) 

*  resthr 

CALL  RECEIVE_REAL_32BIT (  DTACSA(l)  ) 
CALL  RECEIVE_REAL_32BIT (  DTACSA ( 2 )  ) 
CALL  RECEIVE_REAL_32BIT (  DTACSA (3)  J 
CALL  RECEIVE_REAL_32BIT (  DTACSA (4)  ) 
CALL  RECEIVE_R£AL  32BIT(  TATAB  ) 


C - c 

c - TERMINATION  LOGIC - C 

C - C 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

c - c 

C  increment  time 

TSTEP  =  TSTEP  +1.0 
T  -  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  RECEIVE_SIGNED_16BIT (  IEXIT  ) 
'LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
'LOOP'  EPILOGUE 
END 


FILE :  uuv22 . 19g/debug/uublkl0 . for 


PROGRAM  BLK10 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  ALT 
REAL  DELT 
INTEGER  IEXIT 
REAL  PRESS 
REAL  RHO 
REAL  T 
REAL  TSTEP 
REAL  TSTG2 
REAL  VSND 

S INCLUDE ( •' /INCLUDE/SSBLK10. DAT' ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW8  7 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
•LOOP'  START 


CALL  RECEIVE  REAL  32BIT<  ALT  ) 


nnnnnnn 


c 

c 

c 

c 

c 
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ATMOSPHERE  MODULE 


Computes  the  atmospheric  properties 


•C 

■c 

c 

c 

c 

•c 


>7 


IF  (  T.LT.TSTG2  )  THEN 

CALL  ATMOS1 (T, ALT, RHO, PRESS, VSND) 
ENDIF 

CALL  SEND_REAL_32BIT (  PRESS  ) 

CALL  SEND_REAL_32BIT (  RHO  ) 

CALL  SEND  REAL  32BIT(  VSND  ) 


- - c 

TERMINATION  LOGIC  - C 

- C 

Defines  the  simulation  termination  C 

conditions  C 


C 

C 


C  increment  time 

TSTEP  =  TSTEP  >1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  R£CEIVE_SIGNED_16BIT <  IEXIT  ) 

♦LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
♦LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 19g/debug/uublkll .for 


PROGRAM  BLK11 

IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

REAL  AT (3) 

REAL  CEI (9) 

REAL  CG<3) 


DOUBLE 

PRECISION 

CIE (9) 

REAL  CIM (9) 

DOUBLE 

PRECISION 

DELPHI 

DOUBLE 

PRECISION 

DELPSI 

DOUBLE 

PRECISION 

DELT 

DOUBLE 

PRECISION 

DELTHT 

DOUBLE 

PRECISION 

DEL'J 

DOUBLE 

PRECISION 

DELV 

DOUBLE 

PRECISION 

DELW 

DOUBLE 

PRECISION 

DT 

DOUBLE 

PRECISION 

DTEPS 

DOUBLE 

PRECISION 

DTR 

DOUBLE 

PRECISION 

GR  (3) 

INTEGER"!  GYSEED 

INTEGER  IEXIT 

DOUBLE 

r  RECISION 

LATLP 

DOUBLE 

PRECISION 

LONGLP 

REAL  MVR 

DOUBLE 

PRECISION 

MVRDOT 

REAL  P 

REAL  PD 

DOUBLE 

PRECISION 

PHI 

DOUBLE 

PRECISION 

PHIICD 

DOUBLE 

PRECISION 

PSI 

DOUBLE 

PRECISION 

PSIICD 

OOUBLE 

PRECISION 

PULSEA (3) 

REAL  PULSEG (3) 
REAL  Q 


o  o  o  o  o  n 


48 


Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1,  Part  3 


REAL  QD 

DOUBLE  PRECISION  QFRACA ( 3 ) 

DOUBLE  PRECISION  QS1(4) 

REAL  R 
REAL  RD 

DOUBLE  PRECISION  RMII3) 

DOUBLE  PRECISION  RMIRI3) 

REAL  RMIR_ ( 3 ) 

REAL  SP 

DOUBLE  PRECISION  SPHI 
DOUBLE  PRECISION  SPSI 
REAL  SQ 
REAL  SR 

DOUBLE  PRECISION  STHT 
DOUBLE  PRECISION  SU 
DOUBLE  PRECISION  3UD 
DOUBLE  PRECISION  SV 
DOUBLE  PRECISION  SVD 
DOUBLE  PRECISION  SW 
DOUBLE  PRECISION  S.7D 
DOUBLE  PRECISION  T 
DOUBLE  PRECISION  TONAV 
DOUBLE  PRECISION  TACCEL 
DOUBLE  PRECISION  THT 
DOUBLE  PRECISION  THTICD 
REAL  TI2M<9) 

DOUBLE  PRECISION  TIMUDRIV 
DOUBLE  PRECISION  TIMUPR 
DOUBLE  PRECISION  TIMUSTEP 
DOUBLE  PRECISION  TLIMU 
DOUBT E  PRECISION  TNAV 
INTEGER* 4  TOSEED 
DOUBLE  PRECISION  TST20N 
DOUBLE  PRECISION  TSTEP 
DOUBLE  PRECISION  TSTG2 
DOUBLE  PRECISION  TUPLK1 
DOUBLE  PRECISION  TUPLK2 
DOUBLE  PRECISION  UD 
DOUBLE  PRECISION  VD 
DOUBLE  PRECISION  VMII3) 

DOU  I  PRECISION  VMIR ( 3 ) 

REA1  VMIR_<3) 

DOUBLE  PRECISION  VP1 
REAL  VTT ( 3 ) 

DOUBLE  PRECISION  WD 
DOUBLE  PRECISION  X 
DOUBLE  PRECISION  XD 
DOUBLE  PRECISION  XYZE(?' 

DOUBLE  PRECISION  XYZEDI3) 

DOUBLE  PRECISION  Y 
DOUBLE  PRECISION  YD 
DOUBLE  PRECISION  Z 
DOUBLE  PRECISION  ZD 

S INCLUDE ( 1 ' / INCLUDE/SSBLK11 . DAT  1 ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBFR  GENERATOR 

CALL  RANIT  (  TCSEED  ) 


MISSILE  STATE  INITIALIZATION  MODULE  - 


Initialize  integrated  missile  states 


COORDINATE  TRANSFORMATION  MATRICES 

CALL  MM.K  fSNGL  (-9C.  0*DTRI  ,  1 ,  SNGL  (LATLP'DTR)  ,  2, 
S  4GL  (  LONGL?  *  DTR  ,3,CEII 

CIE  M  «  CEI  '1) 

CIE (2)  =  CEI  (4) 

CIE  (3)  =  CEI (7) 

CIE (4)  =  CEI (2) 


a  n  a  n  n  n 


onnonoo 
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CIE (5)  =  CEI (5) 

CIE  {  6)  =  CEI  (8) 

CIE (7)  =  CEI (3) 

CIE  (8)  =  CEI  (6) 

CIE (9)  =  CEI (9) 

C  COMPUTE  MISSILE  STATES  IN  INERTIAL  FRAME 

X  =  XYZE (1) *CEI  (1)  +  XYZE (2) *CEI (9)  +  XYZE (3) "CEI  (7) 

Y  =  XYZE (1) *CEI (2)  +  XYZE (2) "CEI (5)  +  XYZE ( 3 ) "CEI ( 8 ) 

Z  =  XYZE(l) "CEI (3)  +  XYZE (2) "CFI (6)  +  XYZE ( 3 ) "CEI ( 9 ) 

XD  =  XYZED (1) "CEI (1)  +  XYZED (2 ) "CEI ( 4 )  +  XYZED ( 3 ) "CEI ( 7) 

YD  =  XYZED (1) "CEI (2)  +  XYZED (2) "CEI (5)  +■  XYZED ( 3 ) "CEI ( 8 ) 

ZD  =  XYZED (1) "CEI (3)  +  XYZED (2) "CEI ( 6 )  +  XYZED ( 3 ) "CEI ( 9) 

C  INITIAL  MISSILE  EULER  ANGLES  IN  RADIAN. 

PHI  =  PHIICD'DTR 
THT  =  THTICD*DTR 
PSI  -  PSIICD'DTR 

C  ESTIMATED  MISSILE  POSITION  AND  VELOCITY 

RMIR(l)  =  X 
RMIR (2 )  =  Y 
RMIR ( 3 )  =  Z 
VMIR(l)  =  XD 
VMIR (2)  =  YD 
VMIR ( 3 )  =  ZD 

C  ESTIMATED  MISSILE  EULER  ANGLES  AND  BODY  RATES 

SPHI  =  PHI 
STHT  -  THT 
SPSI  =  PSI 

MVR  =  VP1 


MAIN  EXECUTION  LOOP 


Execution  of  all  events  is  performed 
within  this  loop 


100G  CONTINUE 
"LOOP*  START 


CALL  RECEIVE_REAL_64BIT (  XD  ) 
CALL  RECEIVE_REAL_64BIT (  YD  ) 
CALL  RECEIVE  _REAL_64BIT (  ZD  ) 
CALL  RECEIVE_REAL_64BIT (  X  ! 
CALL  RECEIVE_REAL_64BIT (  Y  ) 
CALL  RECEIVE_REAL_64BIT (  Z  ) 
CALL  RECEIVE_REAL_32BIT (  P  ) 
CALL  RECEIVE_REAL_32BIT (  Q  ) 
CALL  RECEIVE_REAL_32BIT (  R  ) 
CALL  RECEIVE_REAL_32BIT (  CIM(l) 
CALL  RECEIVE_REAL_32BIT (  CIMI2) 
CALL  RECEIVE_REAL_32BIT (  CIM(3) 
CALL  RECEIVE_REAL_32BIT (  CIM(4) 
CALL  RECEIVE_REAL_32BIT (  CIM(5) 
CALL  RECEIVE_REAL_32BIT (  CIMI6) 
CALL  RECEIVE_REAL_328IT (  CIM(7) 
CALL  RECEI VE_REAL_32BIT (  CIM{8) 
CALL  RECEIVE  REAL  32BIT (  CIMI9) 


) 

) 

) 

) 

) 

) 

) 

) 

) 


CALL  RECEIVE_REAL_64BIT (  UD  ) 
CALL  RECEIVE_REAL_64BIT (  VD  ) 
CALL  RECEIVE_REAL_64BIT (  WD  ) 
CALL  RECEIVE_R£AL_32BIT (  PD  ) 
CALL  RECEIVE_REAL_32BIT (  CD  ) 
CALL  RECEIVE_REAL_32BIT (  RD  ) 
CALL  RECEIVE_REAL_64BIT (  GR(1)  ) 

CALL  RF,CEIVE_REAL_64BIT  (  GR(2)  ) 
CALL  RECEIVE_REAL_64BIT (  GR ( 3 )  ) 

CALL  RECEIVE  REAL_64BIT (  XYZE(l) 
CALL  RECEIVE  REAL  64BITI  XYZE.  (2) 


) 

) 
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CALL  RECEIVE_REAL_64BIT (  XYZE<3) 
CALL  RECEIVE_REAL_64BIT<  XYZED  (1) 
CALL  RECEIVE_R£AL_64BIT (  XYZED (2) 
CALL  RECEIVE  REAL  64BIT<  XYZED (3) 


) 

) 

) 


CALL  RECEIVE_REAL_32BIT (  CG (1 )  ) 
CALL  RECEIVE_REAL_32BIT (  CG(2)  ) 
CALL  RECEIVE  REAL  32BIT<  CG(3)  ) 


C 

C 

C 

C 

c 

c 

c- 


- c 

INERTIAL  MEASUREMENT  UPDATE  - C 

- c 

Get  inertial  measurement  data  needed  C 

for  guidance  calculations  .  C 

C 

- c 


C 

C 

C 

c 

c 

c 


ACCELEROMETER  MODULE  - 

Determine  sensed  accelerations 


C 

C 

C 

C 

c 

c 


IF  (  TSTEP  .GE.  TIMUDRIV  )  THEN 
TIMUDRIV  =  TIMUDRIV  +  TIMUSTEP 

CALL  ACCEL  (T,UD,VD,WD,P,Q,R,  PD,  QD,RD,CG,CIM,XD,  YD,  ZD, 
GR, GYSEED, QFRACA, PULSEA) 


ENDIF 

CALL  RECEIVE_REAL  32BIT (  PULSEG(l)  ) 
CALL  RECEIVE_REAL_32BIT (  PULSEG(2)  ) 
CALL  RECEIVE_REAL  32BIT {  PULSEG(3)  ) 


C 

C 

C 

C 

C 

C 

c 


- c 

IMU  PROCESSOR  MODULE  - C 

- c 

Convert  gyro  and  accelerometer  outputs  C 

to  delta  angle  and  delta  velocity  C 


C 

■C 


IF  (  TSTEP  .GE.  TIMUDRIV  )  THEN 
TIMUDRIV  =  TIMUDRIV  +  TIMUSTEP 

CALL  IMUPRO (T, PULSEG, PULSFA, DELPHI, DELTHT, DELPSI , 
DELU, DELV, DELW) 


C - c 

C -  NAVIGATION  MODULE  - C 

C - C 

C  This  module  calculates  the  quaternions  C 

C  and  transformation  matrices  using  delta  C 

C  angles  sensed  by  the  gyro  and  calculatesC 

C  the  interceptor  velocity  and  position  C 

C  using  delta  velocity  sensed  by  the  C 

C  accelerometer  C 

C  C 

C - C 


CALL  NAVIG (T, DELPHI , DELTHT, DELPSI, DELU, DELV, DELW, GR, 

QS1 , C1E, SP, SQ, SR, SUD, SVD, SWD, VMIR, RMIR, TI2M, SPHI , STHT, 
SPSI, SU, SV, SW, AT, VMI , RMI , TONAV) 


C  TIME  SINCE  LAST  INERTIAL  MEASUREMENT  UPDATE 

C  DT  -  T  -  TLIMU 

TLIMU  =  T 

DT  =  TIMUSTEP  *  DELT 

C  INTEGRATE  PERFORMANCE  VELOCITY  REMAINING  USING  NAVIGATION 

C  OUTPUT 


nnnnnno 
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IF  (  T.LT.TST20N  .OR.  T.GE.TSTG2  )  THEN 
MVRDOT  =0.0 
ELSE 

MVRDOT  =  -DBLE (SQRT (AT (1)  '*2  +  AT!2)**2  +  AT  <  3 ) *  *  2 )  ) 
ENDIF 


MVR  =  MVR  +  DT*MVRDOT 
IF  (  MVR. LT. 0.0  )  MVR  =  0.0 

C  INTEGRATE  GRAVITY  COMPENSATED  ACCELERATION 


VTT (1) 
VTT (2) 
VTT (3) 


VTT  (1)  +  DT*AT (1 ) 
VTT (2)  +  DT*AT (2) 
VTT (3)  +  DT'AT  (3) 


TACCEL  =  TIMUDRIV  *  DELT 
TIMUPR  =  TIMUDRIV  *  DELT 
TNAV  =  TIMUDRIV  *  DELT 


ENDIF 


MIDCOURSE  CORRECTION 


Models  uplink  of  interceptor, 
target,  and  intercept  conditions 


C 

■C 

■c 

c 

c 

•c 


IF  (  (  DABS (T-TUPLK1) .LE.DTEPS  )  .OR. 

*  (  DABS (T-TUPLK2) .LE.DTEPS  )  )  THEN 

C  REVISE  ESTIMATED  MISSILE  STATES 


VMI (1) 
VMI (2) 
VMI (3) 

RMI (1) 
RMI (2) 
RMI  (3) 

VMIR(l) 
VMIR (2) 
VMIR (3) 

RMIR(l) 
RMIR ( 2 ) 
RMIR  (3) 

T0NAV 

ENDIF 


=  XY2ED(1) 
=  XY2ED (2) 
=  XY2ED ( 3 ) 

=  XYZE(l) 

=  XYZE (2) 

=  XY2E ( 3 ) 

=  XD 
=  YD 
=  ZD 

=  X 
=  Y 
=  Z 

=  T 


RMIR_ ( 1 ) 
RMIR_(2) 
RMIR_<3) 
VMIR_ ( 1 ) 
VMIR_ (2) 
VMIR  (3) 


=  RMIR ( 1 ) 
=  RMIR (2) 
=  RMIR (3) 
=  VMIR ( 1 ) 
=  VMIR (2) 
=  VMIR ( 3 ) 


CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 


SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_64BIT ( 
SEND_REAL_64BIT ( 
SEND_REAL_64BIT ( 
SEND_REAL_32BIT ( 
SEND_R£AL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_64BIT ( 
SEND_REAL_64BIT ( 
SEND_REAL_64BIT ( 
SEND_REAL_32BIT ( 
SEND_R£AL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND  REAL  32BITI 


AT  <1 J  ) 

AT  (2 )  ) 

AT  ( 3 )  ) 

RMIR ( 1 )  ) 

RMIR (2)  ) 
RMIR (3)  ) 
RMIR_(1)  ) 
RMIR_ (2)  ) 
RMIR_<3)  ) 
VMIR(l)  ) 
VMIR  (2 )  ) 

VMIR ( 3 )  ) 

VMI R _ ( 1 )  ) 

VMIR_(2)  ) 
VMIR_(3)  ) 
SP  ) 

SQ  ) 

SR  ) 


onnnooo 
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CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT< 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  S£ND_REAL_32BIT < 
CALL  SEND_REAL  32BIT ( 


TI2M(1)  ) 
TI2M<2)  ) 
TI2M ( 3 )  ) 

TI2M { 4 )  ) 
TI2M ( 5 )  ) 
TI2M  (6)  ) 
TI2M  (7)  ) 

TI2M { 8 )  ) 
TI2MI9)  ) 
MVR  ) 
VTT(l)  ) 
VTT  (2)  ) 

VTT  ( 3  J  ) 


TERMINATION  LOGIC 


Defines  the  simulation  termination 
conditions 


C 

-C 

C 

C 

C 

•C 


C  increment  time 

TSTEP  =  TSTEP  +  1.0D0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED_1 6BIT (  IEXIT  ) 

•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE:  uuv22 . 1 9g/debug/uublkl 2 . for 


PROGRAM  BLK12 


IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (0-2) 


INTEGER  ACQD 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
INTEGER  ESTATE 
REAL  FRMRAT 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
INTEGER  IBURN1 
INTEGER  IEXIT 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  LAMMO (2 ) 
DOUBLE  PRECISION 
INTEGER  MACQ 
REAL  MAGR 
REAL  MAGRTR 
REAL  MAGV 
INTEGER  MCSO 
DOUBLE  PRECISION 
INTEGER  MTERM 
DOUBLE  PRECISION 
REAL  PITER 
REAL  PITERO 
DOUBLE  PRECISION 


ACQRM 

AQRERR 

AQRU 

ASIG 

CMS  (9) 

CSK1 

CSK2 

DELT 

DTEPS 


GRT ( 5 ,  3) 
GRTEST  (3) 


LAM  (2) 
LAMD ( 2 ) 
LAMDXX (2) 

LAMSEK (2) 


MGRDOT 

PI 


RACQ 


onon  oooooo 
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DOUBLE  PRECISION  RMIR(3) 

REAL  RREL ( 3 ) 

REAL  RRELO  1 3 ) 

DOUBLE  PRECISION  RTEST ( 3 ) 

DOUBLE  PRECISION  RTIC(5,  3) 

INTEGER  SEKTYP 
DOUBLE  PRECISION  SKOFF1 
DOUBLE  PRECISION  SKOFF2 
INTEGERS  SKSEED 
DOUBLE  PRECISION  SNRACQ 
REAL  SNRO 

DOUBLE  PRECISION  SSK1 

DOUBLE  PRECISION  SSK2 

DOUBLE  PRECISION  T 

D0U3LE  PRECISION  TAPUDRIV 

DOUBLE  PRECISION  TAPUSTEP 

INTEGER  TERM 

REAL  TGE1 

REAL  TGE2AL 

REAL  TGIL 

REAL  TGO 

DOUBLE  PRECISION  TGPUDRIV 
DOUBLE  PRECISION  TGPUSTEP 
REAL  TI2M (9) 

REAL  TI2MO (9) 

DOUBLE  PRECISION  TKFUDRIV 
DOUBLE  PRECISION  TKFUSTEP 
DOUBLE  PRECISION  TKVON 
DOUBLE  PRECISION  TL2 
INTEGER*  <1  TOSEED 
INTEGER  TRACK 
REAL  TRMTGO 
DOUBLE  PRECISION  TSTEP 
DOUBLE  PRECISION  TUPLK1 
DOUBLE  PRECISION  TUPLK2 
REAL  URREL (3) 

DOUBLE  PRECISION  VMIR(3) 

REAL  VREL  (3) 

REAL  VRELO (3) 

DOUBLE  PRECISION  VTEST<3) 

DOUBLE  PRECISION  VTIC<5,  3) 

DOUBLE  PRECISION  WFILT 
REAL  YAWER 
REAL  YAWERO 

DOUBLE  PRECISION  ZFILT 

SINCLUDE  (  '  ’VINCLUDE/SSBLK12  .  DAT  '  ) 

'LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


- c 

MISSILE  STATE  INITIALIZATION  MODULE  - C 

- c 

Initialize  integrated  missile  states  C 

C 

- c 


MISSILE  TO  SEEKER  MATRIX  (  INCLUDES  MISALIGNMENT  ) 

SEEKER  MISALIGNMENT  DIRECTIONS  : 

SKOFF1  =  CONE  ANGLE  OFF  NORMAL  (  CURRENTLY  UNDEFINED  ) 
SKOFF2  =  POLAR  ANGLE 


C  NOTE:  TRANSFORMATION  INCLUDES  180  DEGREE  ROTATION  ABOUT  Y-AXIS 

SKOFF1  =0.0 

SKOFF2  =  2 . 0*PI*RAN0 (TOSEED) 

CSK1  =  DCOS ( SKOFF1 ) 

SSK1  =  DS IN (SKOFF1 ) 

CSK2  =  DCOS ( SKOFF2 ) 

SSK2  =  DSIN ( SK0FF2 ) 

CMS ( 1 )  =  -CSK1 


nnonnno 
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CMS (2)  =  SSK1 *CSK2 
CMS (3)  =  SSK1*SSK2 
CMS (4)  =  SSK1*SSK2 
CMS (5)  =  CSK1 
CMS (6)  =  SSK1*CSK2 
CMS (7)  =  SSK1*CSK2 
CMS (8)  =  SSK1»SSK2 
CMS (9)  =  -CSK1 

C  INITIALIZE  SEEKER  PARAMETERS 

CALL  NORM (AQRU, 0 . 0D0, SKSEED, AQRERR) 
RACQ  =  ACQRM  +  AQRERR 


MAIN  EXECUTION  LOOP 


Execution  of  all  events  is  performed 
within  this  loop 


1000  CONTINUE 
•LOOP*  START 


★ 


CALL  RECEIVE 
CALL  RECEIVE 
CALL  RECEIVE' 


REAL 

"REAL" 

■real' 


64BIT ( 
64BIT ( 
643IT ( 


GRT  (1,1)  ) 
GRT  (1,2)  ) 
GRT  (1,3)  ) 


CALL  RECEIVE_REAL_84BIT(  GRT (2,1) 
CALL  RECEIVE_REAL_64BIT (  GRT (2, 2) 
CALL  R£CEIVE_REAL_64BIT(  GRT (2, 3) 
CALL  REC£IVE_REAL_64BIT (  GRT (3,1) 
CALL  REC£IVE_REAL_64BIT(  GRT (3, 2) 
CALL  REC£IVE_REAL_64BIT (  GRT (3, 3) 
CALL  RECEIVE_REAL_64BIT (  GRT (4,1) 
CALL  RECEIVE_REAL_64BIT <  GRT (4, 2) 
CALL  RECEIVE_REAL_64BIT(  GRT (4, 3) 
CALL  RECEIVE_REAL_64BIT<  GRT (5,1) 
CALL  RECEIVE_REAL~64BIT (  GRT (5, 2) 
CALL  RECEIVE  REAL  64BIT (  GRT (5, 3) 
CALL  RECEIVE_REAL  64BIT(  VTIC (1,1) 
CALL  RECEIVE_REAL_64BIT<  VTIC(1,2) 
CALL  RECEIVE_REAL_64BIT (  VTIC(1,3) 
CALL  RECEIVE_REAL_64BIT(  VTIC(2,1) 
RECE I VE_REAL_6  4  B I T ( 

RECE I VE_REAL_6  4  BIT ( 
RECET'/E_°EAL_64BIT  ( 
RECEIVE_REAL_64BIT ( 

RECE I VE_RE AL_6  4  BIT ( 
RECEIVE_REAL_64BIT ( 
RECEIVE_REAL_64BIT ( 
RECEIVE_REAL_64BIT ( 
RECEIVE_REAL_64BIT ( 

RECE I VE_REAL_6  4B I T ( 
RECEIVE_REAL_64BIT ( 

RECE I VE_REAL  64BIT( 

RECEIVE  REAL~64BIT< 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 

CALL 

CALL 

CALL  RECEIVE_REAL_64BIT (  RTIC(1,3)  ) 
CALL  RECEIVE_REAL_64BIT (  RTIC(2,1) 
CALL  RECEIVE_REAL_64BIT (  RTIC(2,2) 
CALL  RECEIVE_REAL_64BIT (  RTIC (2, 3) 
CALL  RECEIVE_REAL_64BIT (  RTIC (3,1) 
CALL  RECEIVE_REAL_64BIT (  RTIC (3, 2) 
CALL  RECEIVE_REAL_64BIT (  RTIC (3, 3) 
CALL  RECEIVE_REAL_64BIT (  RTIC (4,1) 
CALL  RECEIVE_REAL_64BIT (  RTIC (4, 2) 
CALL  RECEIVE_REAL_64BIT (  RTIC (4, 3) 
CALL  RECEIVE_REAL_64BIT (  RTI~(S,1> 
CALL  RECEIVE_REAL_64BIT (  RTIi_(S,2) 
CALL  RECEIVE  REAL  64BIT (  RTIC (5, 3) 


VTIC (2,2) 
VTIC  (2,3) 
VTIC  (3, 1) 
VTIC  (3,2) 
VTIC  (3, 3) 
VTIC (4, 1) 
VTIC  (4,2) 
VTIC (4, 3) 
VTIC  (5,1) 
VTIC (5, 2) 
VTIC (5, 3) 
RTIC  (1,1)  ) 
RTIC  (1, 2)  ) 


) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 


) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 


- - - 

c - MIDCOURSE  CORRECTION  - C 

c - c 

c  Models  Uplink  of  interceptor,  C 

C  target,  and  intercept  conditions  C 

C  "  C 

- - - 


u  o  o  o  u  o  u 


nooonoo 
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IF  (  (  DABS (T-TUPLK1) .LE.DTEPS  )  .OR. 

*  (  DABS (T-TUPLK2) .LE.DTEPS  )  )  THEN 

C  REVISE  ESTIMATED  TARGET  STATES 


RTEST (1) 
RTEST (2) 
RTEST (3) 

VTEST  (1) 
VTEST (2) 
VTEST (3) 

GRTEST  (1) 
GRTEST (2) 
GRTEST (3) 

TL2 


=  RTIC  (1, 1) 
=  RTIC  (1 , 2 ) 
=  RTIC (1, 3) 

=  VTIC  (1,1) 
=  VTIC (1, 2) 
=  VTIC (1,3) 

=  GRT  (1,1) 

=  GRT  (1, 2) 

=  GRT (1, 3) 

=  T 


ENDIF 


- c 

ON  BOARD  TARGET  MODULE  - C 

- c 

Estimate  target  position  based  on  C 

predicted  intercept  conditions  C 

C 

- c 


IF  (  TSTEP  . GE .  TGPUDRIV  )  THEN 

*  TGPUDRIV  =  TGPUDRIV  +  TGPUSTEP 

C  GRTEST  TEMPORARILY  EQUAL  TO  GRT 

GRTEST (1)  =  GRT (1, 1) 

GRTEST (2)  =  GRT (1,2) 

GRTEST  (3)  =  GRT  (1,3) 

CALL  OBTARG (T, GRTEST, RTEST, VTEST, TL2) 

ENDIF 


CALL  RECEIVE_REAL_32BIT(  MAGRTR  ) 
CALL  RECEIVE_REAL_64BIT (  LAMDXX(l)  ) 
CALL  RECEIVE  REAL  64BIT<  LAMDXX(2)  ) 


CALL  RECEIVE_REAL_64BIT (  RMIR(l) 
CALL  RECEIVE_REAL_64BIT (  RMIR(2) 
CALL  RECEIVE_REAL_6<IBIT  (  RMIR(3) 
CALL  RECEIVE_REAL_6‘!BIT  (  VMIR(l) 
CALL  RECEIVE_REAL_64BIT (  VMIR(2) 
CALL  RECEIVE_REAL_6‘tBIT  (  VMIR(3) 
CALL  RECEIVE_REAL_32BIT (  TI2M(1) 
CALL  RECEIVE_REAL_32BIT (  TI2M(2) 
CALL  RECEIVE_REAL_32BIT (  TI2M(3) 
CALL  RECEIVE_REAL_32BIT (  TI2M(4) 
CALL  RECEIVE_REAL_32BIT (  TI2M<5) 
CALL  RECEIVE_REAL_32BIT(  TI2M(6) 
CALL  RECEIVE_REAL_32BIT (  TI2M<7) 
CALL  RECEIVE_REAL_32BIT (  TI2M(8) 
CALL  RECEIVE  REAL  32BIT (  TI2M<9) 


) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 


c - C 

C -  ESTIMATED  RELATIVE  STATES  MODULE  - C 

C - C 

C  Estimate  range,  range  rate,  and  time-to-  C 

C  go  based  on  navigation  output  and  target  C 

C  model  estimates  C 

C  C 

c - c 


IF  (  TSTEP  .GE.  TGPUDRIV  )  THEN 
TGPUDRIV  =  TGPUDRIV  +  TGPUSTEP 

CALL  ESTREL (RTEST, VTEST, RMIR, VMIR, TI2M,CMS, ESTATE, RREL, VREL, 
MAGR, MAGV, URREL, MGRDOT, TGO, PITER, YAWER, LAMD) 


ENDIF 


o  o  n  o  o  o 
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PITERO  =  PITER 
YAWERO  -  YAWER 


CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 


SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
S£ND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT { 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT< 
SEND_REAL_32BIT ( 
SEND_REAL_32BIT ( 
SEND  REAL  32BIT ( 


URREL(l)  ) 
URREL (2)  ) 
URREL  (3)  ) 
RREL(l)  ) 
RREL ( 2 )  ) 
RREL ( 3 )  ) 
VREL(l)  ) 
VREL  (2)  ) 
VREL ( 3  >  ) 
TGO  ) 

MAGR  ) 

MAGV  ) 
PITERO  ) 
YAWERO  ) 


CALL  SEND_SIGNED_1 6BIT (  ACQD  ) 
CALL  RECEIVE  R£AL_32BIT (  FRMRAT  ) 


CALL  RECE I VE_REAL_3  2  B I T ( 
CALL  RECEIVE_REAL_32BIT( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT< 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  REC£IVE_REAL_32BIT( 
CALL  RECEIVE_REAL_32BIT( 
CALL  RECEIVE_REAL  32BIT ( 
CALL  RECEIV£_REAL_32BIT ( 
CALL  RECEIVE__REAL_32BIT  ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECE I VE_RE AL  32BIT ( 
CALL  RECEIVE_REAL— 32BIT ( 
CALL  RECEIVE_REAL_32BIT( 
CALL  RECEIVE  REAL_32BIT ( 


LAMMO(l)  ) 
LAMMO  ( 2 )  ) 
RRELO(l)  ) 
RRELO  (2)  ) 
RRELO ( 3 )  ) 
SNRO  ) 
TI2MO { 1 )  ) 
TI2MO (2)  ) 
TI2MO (3)  ) 
TI2MO  ( 4 )  ) 
TI2MO (5)  ) 
TI2MO (6)  ) 
TI2MO (7)  ) 
TI2MO  (8)  ) 
TI2MO (9)  ) 
VRELO(l)  ) 
VRELO (2)  ) 
VRELO (3)  ) 


KALMAN  FILTER  MODULE 


Filter  LOS  angles 


IF  (  TSTEP  .GE.  TKFUDRIV  )  THEN 

TKFUDRIV  «  TKFUDRIV  +  TKFUSTEP 

C  CALL  FILTER  IF  SNR  IS  SUFFICxe-w  1 

IF  (  SNRO . GE . SNRACQ  .OR.  SEKTYP . NE . 2  )  THEN 

IF  (SEKTYP. EQ.l  -OR.  SEKTYP. EQ. 2)  THEN 
ASIG  =  (32. S6*SNRO** (-0.29912) ) *1.0E-6 
ENDIF 

CALL  KALMAN (T, TI2M,  LAMMO,  ASIG,  SNRO,  TGO,  RRELO,  VRELO, 

.  TI2MO, RACQ, MAGRTR, MAGR, MAGV, LAMSEK, LAMDXX, FRMRAT, CMS, 

.  MACQ, MCSO, MTERM, TRACK, TERM, TRMTGO, TGE1 , 

TGE2AL, WFILT, ZFILT, LAM, LAMD, IBURN1, ACQD, ESTATE, 
PITER, YAWER, TGIL) 


ENDIF 

ENDIF 


CALL  SEND_REAL_32BIT (  TGIL  ) 

CALL  SEND_REAL_32BIT (  PITER  ) 
CALL  SEND_REAL_32BIT (  YAWER  ) 
CALL  SEND_REAL_64BIT (  LAMD ( 1 )  ) 
CALL  SEND_REAL_64BIT (  LAMD (2)  ) 
CALL  SEND_REAL_32BIT (  TRMTGO  ) 
CALL  SEND_REAL_32BIT (  TGE1  ) 

CALL  SEND_REAL_32BIT (  TGE2AL  ) 
CALL  SEND_SIGNED_16BIT(  IBURN1  ) 
CALL  SEND  SIGNED  16BIT<  ESTATE  ) 


o  o  o  o  n  o 
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IF  {  TSTEP . GE . TAPUDRI V  )  THEN 

TAPUDRIV  =  TAPUDRIV  +  TAPUSTEP 

IF  (  T.GE.TKVON  )  THEN 

IF  (  TGO.LE.TGE1  .AND.  IBURN1.EQ.0  )  THEN 
*  The  IBURN1  assignment  was  moved  from  the  partition  with  VCSLOG 
IBURN1  «  1 
ENDIF 
ENDIF 
ENDIF 


C - C 

C -  TERMINATION  LOGIC - C 

C - C 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

C - C 

C  increment  time 

TSTEP  =  TSTEP  +  1.0D0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  RECEIVE_STGNED_16BIT (  IEXIT  ) 
*LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE:  uuv22.19g/debug/uublX13. for 


PROGRAM  BLK13 

IMPLICIT  REAL 

IMPLICIT  REAL 

REAL 

ALFAP 

REAL 

ALFAT 

REAL 

ALFAY 

REAL 

CA 

REAL 

CG  ( 3 ) 

REAL 

CN 

REAL 

DELT 

REAL 

DTEPS 

REAL 

FXA 

REAL 

FYA 

REAL 

FZA 

INTEGER  IAERO 

INTEGER  IEXIT 

REAL 

MACH 

REAL 

MVRWM 

REAL 

MXA 

REAL 

MYA 

REAL 

MZA 

REAL 

QA 

REAL 

RHO 

REAL 

T 

REAL 

TBRK 

INTEGER*  A  TOSEED 

REAL 

TSTEP 

REAL 

TSTG1 

REAL 

TSTG2 

REAL 

VRWM ( 3 ) 

REAL 

VSND 

REAL 

XCP 

S INCLUDE ('"/INCLUDE/SSBLK13.DAT’ ) 


LOOP*  PROLOGUE 


onnnnon  *  nnnnonn 
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*  INITIALIZE  80x87 

CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 

*  initialization  for  purpose  of  delaying  receipt  of  actual  values 

RHO  =  0.23769E-02 

VSND  =  1116.45 

VRWM(l)  =0.0 
VRWM { 2 )  =0.0 
VRWM ( 3 )  =0.0 
MVRWM  =0.0 


-  - c 

MAIN  EXECUTION  LOOP - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
LOOP*  START 

CALL  RECEIVE_REAL_32BIT (  CG(1)  ) 
CALL  RECEIVE_REAL_32BIT (  CG(2)  ) 
CALL  RECEIVE  REAL  32BIT(  CG(3)  ) 


- c 

AERODYNAMICS  MODULE  - C 

- c 

Computes  the  aerodynamic  forces  and  C 

moments  C 

C 

- c 


IF  (  T.LE. (TSTG2+DTEPS)  )  THEN 

CALL  AERO  <T( VRWM, CG, MVRWM, RHO, VSND, I AERO, TBRK, QA, MACH, 

al f at , ALFAP , ALFAY, CA, CN, XCP , FXA, FYA, FZA, MX A, MYA, MZA) 

ENDIF 

IF  (  ABS (T-TSTG1) .LE.DTEPS  )  THEN 
I AERO  =  1 
ENDIF 

*  These  values  are  delayed  so  that  AERO  can  run  sooner 

CALL  RECEIVE_REAL_32BIT (  RHO  ) 

CALL  RECEIVE_REAL_32BIT (  VSND  ) 

CALL  SEND_REAL_32BIT (  MACH  ) 

CALL  SEND_REAL_32BIT (  QA  ) 

CALL  SEND_REAL_32BIT (  FXA  ) 

CALL  SEND_REAL_32BIT (  FYA  ) 

CALL  SEND_REAL_32BIT (  FZA  ) 

CALL  SEND_REAL_32BIT (  MX A  ) 

CALL  SEND_REAL_32BIT (  MYA  ) 

CALL  SEND_REAL_32BIT (  MZA  ) 

*  These  values  are  delayed  so  that  AERO  can  run  sooner 

CALL  RECEIVE_REAL_32BIT (  VRWM ( 1 )  ) 

CALL  RECEIVE_REAL_32BIT (  VRWM (2)  ) 

CALL  RECEIVE  REAL_32BIT (  VRWM (3)  ) 

CALL  RECEIVE~REAL  32BIT (  MVRWM  ) 


c - c 

c - TERMINATION  LOGIC  - C 

C - C 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

c - c 

C  increment  time 

TSTEP  =  TSTEP  +  1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


nnnoonn 
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CALL  RECEIVE_SIGNED_16BIT (  IEXIT  ) 
•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE:  uuv22 . 1 9g/debug/uublkl 4 . for 


PROGRAM  BLK14 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  ACS LEV 
REAL  BFXACS 
REAL  BFYACS 
REAL  BFZACS 
REAL  BMDOTA 
REAL  BMXACS 
REAL  BMYACS 
REAL  BMZACS 
REAL  CG ( 3 ) 

REAL  DELT 
REAL  DTACSB ( 4 ) 

INTEGER  IACSONB 
INTEGER  IEXIT 
INTEGER  ITHRES 
REAL  T 
REAL  TATAB 
REAL  TKVON 
INTEGER* 4  TOSEED 
REAL  TSTEP 

SINCLUDE  ( *  'VINCLUDE/SSBLK1  4  .  DAT  ‘  ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 

within  this  loop  C 

C 

- c 


1000  CONTINUE 
•LOOP*  START 

C  from  MASSPR 

CALL  RECEIVE_REAL_32BIT (  CG(i)  ) 
CALL  RECEIVE_REAL_32BIT <  CG(2)  ) 

CALL  RECEIVE  REAL_32BIT (  CG ( 3 )  ) 


C 

C 

C 

C 

C 

C 

C 


- c 

ACS  THRUSTER  RESPONSE  MODULE  - C 

- c 

Determines  the  forces  and  moments  C 

imparted  by  the  ACS  thrusters  C 


C 

■C 


IF  (  T.GE. TKVON  )  THEN 

CALL  ACSTHB IT, CG, ACSLEV, DTACSB, TATAB, TOSEED, 

ITHRES, BFXACS, BFYACS, BFZACS, BMXACS, BMYACS, BMZACS, 
BMDOTA, IACSONB) 


60 


Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1,  Part  3 


ENDIF 

CALL  SEND_REAL_32BIT (  BFXACS  ) 

CALL  SEND_REAL_32BIT (  BFYACS  ) 

CALL  SEND_REAL_32BIT (  BFZACS  ) 

CALL  SEND_R£AL_32BIT (  BMXACS  ) 

CALL  SEND_REAL_32BIT (  BMYACS  ) 

CALL  SEND_REAL_32BIT (  BMZACS  ) 

CALL  SEND_REAL_32BIT {  BMDOTA  ) 

CALL  S£ND_SIGNED_1 6BIT (  IACSONB  ) 

*  Kvauto 

CALL  R£CEIVE_REAL_32BIT (  ACSLEV  ) 
CALL  REC£IVE_SIGNED_16BIT (  ITHRES  ) 

*  resthr 

CALL  RECEIVE_REAL_32BIT (  DTACSB(l)  ) 
CALL  RECEIVE_REAL_32BIT (  DTACSB(2)  ) 
CALL  RECEIVE_REAL_32BIT(  DTACSB(3)  ) 
CALL  RECEIVE_REAL_32BIT (  DTACSB<4)  ) 
CALL  RECEIVE  REAL  32BIT (  TATAB  ) 


C - C 

c - TERMINATION  LOGIC - C 

c - c 

C  Defines  the  simulation  termination  C 

C  conditions  C 

C  C 

c - c 

C  increment  time 

TSTEP  »  TSTEP  +1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 


CALL  RECEIVE_SIGNED_16BIT<  IEXIT  ) 
•LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
•LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 1 9g/debuq/uublkl5 . for 


PROGRAM  BLK15 

IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

INTEGER  ACQD 
REAL  ACSLEV 
REAL  ADISTT (4,  3) 

DOUBLE  PRECISION  ANVP 
DOUBLE  PRECISION  DELT 
REAL  DTACSA ( 4 ) 

REAL  DTACSB ( 4 ) 

DOUBLE  PRECISION  DTEPS 
REAL  DTOFFV ( 4 ) 

DOUBLE  PRECISION  D  f SAMP 
DOUBLE  PRECISION  DTVCSP(3) 

DOUBLE  PRECISION  DTVCSY (3) 

DOUBLE  PRECISION  FLTC ( 4 ) 

DOUBLE  PRECISION  FLTCP 
DOUBLE  PRECISION  FLTCY 
INTEGER  IACSON 
INTEGER  IACSONA 
INTEGER  IACSONB 
INTEGER  IBURN1 
INTEGER  IBURN2 
INTEGER  IBURN3 
INTEGER  IBURND 
INTEGER  IBURNM 
INTEGER  ICMD 
INTEGER  IDIST 
INTEGER  IDMEAS 
INTEGER  IDROP 


o  o  o  o  o  o  o 
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INTEGER  IEXIT 
INTEGER  IPASSM 
INTEGER  ITHRES 
INTEGER  IVCS 
INTEGER  IVTAB 
REAL  IXX 
REAL  IYY 
REAL  I2Z 

DOUBLE  PRECISION 
REAL  MAGV 
DOUBLE  PRECISION 
INTEGER  MIDBRN 
REAL  PITER 
REAL  ROLLER 
REAL  SP 
REAL  SQ 
REAL  SR 

DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  _  RECISION 
REAL  TATAB 
REAL  TBURNM 
DOUBLE  PRECTCION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  TGE1 
DOUBLE  PRECISION 
REAL  TGE2AL 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  TGIL 
REAL  TGO 

DOUBLE  PRECISION 
REAL  TIMONV 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  TOFFLT (4) 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  TRMTGO 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL  TVTAB 
DOUBLE  PRECISION 
REAL  VGMI3) 

REAL  YAWER 


LAMD (2) 
MASS 


SW80 

T 

TAPUDRIV 

TAPUSTEP 


TBURNP 

TBURNY 

TCMINV 

TCWAIT 

TDROP 

TGE2 

TGI  IP 
TGI1Y 
TGI2P 
TGI2Y 
TGI3P 
TGI3Y 


TGOFLM 

TKVON 

TLAPS 

TMAUTO 

TNEXT 

TOFLTM 

TPATON 

TRATON 

TSAH 

TSAL 

TSTEP 

TSTG2 

TYATON 


$  INCLUDE ( * " /INCLUDE/SSBLK1 5 . DAT ' ) 
♦LOOP*  PROLOGUE 


*  INITIALIZE  80x87 
CALL  CW8  7 


- C 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
♦LOOP*  START 

CALL  R£CEIVE_REAL_6i!3I7  (  MASS  ) 
CALL  RECEIVE  REAL  32BIT (  IXX  ) 


onnno 
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CALL  RECEIVE_REAL_32BIT (  IYY  ) 

CALL  RECEIVE_REAL_32BIT (  IZZ  ) 

CALL  RECEIVE_SIGNED_163IT (  IACSONA  ' 
CALL  RECEIVE_SIGNED_1 6BIT {  IACSONB  ) 
CALL  RECEIVE_REAL_32BIT (  SP  ) 

CALL  RECEIVE_R£AL_32BIT (  SQ  ) 

CALL  RECEIVE_REAL_J2BIT (  SR  ) 

CALL  RECEIVE_REAL_32BIT (  TGO  ) 

CALL  RECEIVE_REAL_32BIT (  MAGV  ) 

CALL  RECEIVE_SIGNED_1 6BIT (  ACQD  ) 

CALL  RECEIVE_SIGNED_16BIT (  IDROP  ) 

CALL  RECEIVE_SIGNED_16BIT.<  IBURND  ) 
CALL  RECEIVE_SIGNED_1 6BIT {  IBURNM  ) 
CALL  RECEIVE_SIGNED_16BIt (  IDMEAS  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (1,1)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (1,2)  ) 

CALL  RECE:''E_REAL_32BIT  (  ADISTT  (1,3)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (2,1)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (2, 2)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (2, 3)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (3,1)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (3, 2)  ) 

CALL  RECEIVE_REAL_32BIT (  ADISTT (3, 3)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (4,1)  ) 
CALL  RECEIVE_REAL_32EIT (  ADISTT (4, 2)  ) 
CALL  RECEIVE_REAL_32BIT (  ADISTT (4, 3)  ) 
CALL  RECEIVE_REAL_32BIT (  VGM(l)  ) 

CALL  RECEIVE_REAL_32BIT (  VGM(2)  ) 

CALL  RECEIVE_REAL_32BIT (  VGM<3)  ) 

CALL  RECEIVE_SIGNED_16BIT (  IVCS  ) 

CALL  RECEIVE_REAL_32BIT (  TGIL  ) 

CALL  RECEIVE_REAL_32BIT (  PITER  ) 

CALL  RECEIVE_R£AL_32BIT<  YAWER  ) 

CALL  RECEIVE_REAL_64BIT (  LAMD(l)  ) 

CALL  RECEIVE_REAL_64BIT (  LAMD ( 2 )  ) 

CALL  R£CEIVE_REAL~32BIT(  TRMTGO  ) 

CALL  RECEIVE_REAL_32BIT (  TGE1  ) 

CALL  RECEIVE_REAL_32BIT (  TGE2AL  ) 

CALL  RECEIVE_SIGNED_16BIT (  IBURN1  ) 
CALL  RECE I VE_REAL  32BIT (  ROLLER  ) 


IF  (  TSTEP  .  Gl.  TAPUDRIV  )  THEN 
TAPUDRIV  -  TAPUDRIV  +  TAPUSTEP 
IF  (  T.GE.TKVON  )  THEN 

CALL  VCSTH2 (T, FLTC, FLTCP, FLTCY, TOFFLT,  TIMCNV) 

ENDIF 

ENDIF 

-  - c 

-  AUTOPILOTS  -  ■  -  - C 

-  .  - - C 

C 


IF  (  TSTEP  . GE .  TAPUDRIV  )  THEN 
TAPUDRIV  -  TAPUDRIV  +  TAPUSTEP 


MIDCOURSE  AUTOPILOT  MODULE 


Performs  large  angle  rear  ier.ts  and  ratio  C 
control  auring  mi  occurs**  C 


IF  (  T.GE.TKVON  )  THEN 

*  (above)  CALL  VCSTH1  (T,  FLTC,  FL'r~P,  FLTCY,  TOFFLT,  TIMCNV) 

IF  <  T.GT.T3TG 2  .AND.  T.GE. "MAUTO  .AND. 

(  ICMD.NE.O  .OR.  ACQD.EQ.O)  )  THEN 

C  NOSE  FAIRING  /  BOOST  ADAPTER  SEP-ht>AT  1  ( 


noonoonn 
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IF  (  IDROP.EQ.l  .OR.  (DABS (T-TDRCP) . LE . DTEPS )  )  THEN 
IPASSM  =  0 
ENDIF 

IF  (  (  IACSONA  .EQ.  1  )  OR.  (  IACSONB  .EQ.  1  )  )  THEN 

IACSON  =  1 
ELSE 

IACSON  =  0 
ENDIF 

CALL  MCAUTQ (T, IXX, IYY, IZZ, SP, SQ, SR, ROLLER,  PITER, 

YAWER,  IDIST,  IACSON,  IBURND,  IBURNM,  IDMEAS,  IPASSK, 
ICMD, TRATON, TPATON, TYATON, CTSAMP , TSAL, TSAH, 
TLAPS, ITHRES, ANVP, ACSLEV, TMAUTO) 


ENDIF 


c - C 

C - KV  AUTOPILOT  MODULE  - C 

C - C 

C  Calls  the  various  ACS  autopilot  C 

C  modes  used  for  controlling  the  C 

C  kill  vehicle  attitude  during  flight.  C 

C  Its  purpose  is  to  define  which  C 

C  thruster  to  burn,  for  how  long,  and  at  C 

C  what  thrust  level.  C 

C  C 

c - c 


CALL  KVAUTO (T, SP, SQ  SR, FLTCP, FLTCY, IXX, IYY, IZZ, ADISTT, 

ROLLER, PITER, YAWER, TCWAIT, IDIST, SW80, TSAL, TSAH, 
TNEXT, TLAPS, ANVP, DTSAMP, ACSLEV, TRATON, TPATON, 
TYATON, ITHRES) 


VCS  LOGIC  MODULE 


Controls  the  kill  vehicle  velocity  by 
determining  the  appropriate  VCS  thruster 
on  and  off  times. 


CALL  VCSLOG (T, MASS, LAMD, TGO, MAGV, TGIL, TRMTGC, TGE2AL, 

TGE1 , VGM, IVCS, IDMEAS, IBURNM, MIDBRN, IBURN1, IBURN2, 

IBURN3,  IDIST,  FLTC.  FLTCP.,  FLTCY,  TSAL,  TSAH,  TOFFLT , 
TOFLTM,  TBURNP, T3URNY, TGE2 , TGI IP, TGI2P, TGI 3P , 
TGI1Y, TGI2Y, TGI3Y, TIMONV, TGOFLM, TCWAIT , DTVCSP , 
DTVCSY, DTCFFV, TBURNM) 

SET  FLAG  TO  COMPUTE  VCS  THRUSTER  RESPONSE  TABLE 

IVTAB  =  1 
TVTAB  =  T 


C - C 

C - ACS  RESOLVING  LOGIC  MODULE - C 

c - c 

C  C 

c - c 


IF  (  ITHRES.EQ.l  )  THEN 

CALL  RESTHR (T, IDIST, ANVP, DTSAMP, TOFLTM, TRATON, 
TPATON, TYATON, DTACSA, DTACSB) 

BEGINNING  TIME  OF  ACS  THRUSTER  RESPONSE  TABLE 

TATAB  =  T 

ENDIF 


ENDIF 


onoooono 
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*  kvauto 

CALL  SEND_REAL_32BIT (  ACSLEV  ) 
CALL  SEND_SIGNED_16BIT(  ITHRES  ) 

*  vcslog 

CALL  SEND_REAL_32BIT t  DTOFFV(l)  ) 
CALL  SEND_R£AL_32BIT I  DT0FFV(2)  ) 
CALL  SEND_REAL_32BIT (  DT0FFV<3)  ) 
CALL  SEND_REAL_32BIT (  DTOFFV ( 4 )  ) 

CALL  SEND_SIGNED_1 6BIT (  IVTAB  ) 
CALL  SEND_REAL_32BIT (  TFURNM  ) 
CALL  SEND_REAL_32BIT (  T' MONV  ) 
CALL  SEND_REAL_32BIT  (  TtFFLT(l)  ) 
CALL  SEND_REAL_32BIT (  Tt  FFLT  ( 2  )  ) 
CALL  SEND_REAL_32BIT (  TCFFLT ( 3 )  ) 
CALL  SEND_REAL_32BIT (  TOFFLT ( 4 )  ) 
CALL  SEND_REAL_32BIT (  TVTAB  ) 

'  resthr 

CALL  SEND_REAL_32BIT <  DTACSA ( 1 )  ) 
CALL  SEND_REAL_32BIT (  DTACSA (2)  ) 
CALL  SEND_REAL_32BIT (  DTACSA (3)  ) 
CALL  SEND_REAL_32BIT (  DTACSA (4)  ) 
CALL  SEND_REAL_32BIT (  DTACSB(l)  ) 
CALL  SEND_REAL_32BIT (  0TACSB<2>  ) 
CALL  SEND_REAL_32BIT (  DTACSB<3)  ) 
CALL  SEND_REAL_32BIT (  DTACSB(4>  ) 
CALL  SEND_REAL_32BIT (  TATAB  ) 

*  needed  by  blkOl  only 

CALL  SEND_SIGNED_1 6BIT (  MIDBRN  ) 
CALL  SEND_SIGNED  16BIT(  ICMD  ) 
CALL  SEND  SIGNED  16BIT(  IDIST  ) 


IF  (  T.GE.TKVON  )  THEN 

'  The  ITHRES  assignment  was  moved  from  the  partition  with  ACSTHR 
ITHRES=0 

IF  (IVTAB  .EQ.  1)  THEN 

*  The  IVTAB  assignment  was  moved  from  the  partition  with  VCSTHR 
IVTAB  =  0 

IF (TBURNM.GE.TCMINVI  T8URNM=0.0 
ENDIF 

ENDIF 


- c 

TERMINATION  LOGIC - C 

- C 

Defines  the  simulation  termination  C 

conditions  C 

C 

- C 


C  increment  time 

TSTEP  =  TSTEP  +  1.0D0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED_16BIT (  IEXIT  ) 

♦LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
♦LOOP*  EPILOGUE 
END 


FILE :  Uuv22 . 1 9g / debug /uub 1 kl 6 . for 


PROGRAM  BLK16 


IMPLICIT  REAL 
IMPLICI'  REAL 


(A-H) 

(O-Z) 


o  n  n  o  n  o 
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INTEGER  ACQD 
REAL  ACQRM 
REAL  AQRERR 
REAL  AQRU 
REAL  DELT 
INTEGER  FRMCNT 
REAL  FRMRAT 
INTEGER  IEXIT 
INTEGER  KFSF 
REAL  LAMM (2) 

REAL  LAMMO ( 2 ) 

REAL  LAMSEK ( 2 ) 

INTEGER  LATCH 
REAL  MAGRTR 
REAL  RACQ 
REAL  RREL ( 3 ) 

REAL  RRELO ( 3 ) 

REAL  SAMACQ 
REAL  SAMRAT 
INTEGER* 4  SKSEED 
REAL  SNR 
REAL  SNRO 
REAL  T 

INTEGER  TERM 
REAL  TI2M { 9) 

REAL  TI2MO ( 9) 

REAL  TKFU 
REAL  TKFUDRIV 
REAL  TKFUSTEP 
INTEGER*4  TOSEED 
INTEGER  TRACK 
REAL  TSPUDRIV 
REAL  TSPUSTEP 
REAL  TSTEP 
REAL  VREL ( 3 ) 

REAL  VRELO ( 3 ) 

$  INCLUDE  (  1  "'/INCLUDE/SSBLK16.  DAT  *  ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


MISSILE  STATE  INITIALIZATION  MODULE  - 

Initialize  integrated  missile  states 


C  INITIALIZE  SEEKER  PARAMETERS 

SAMRAT  =  SAMACQ 

CALL  NORM (AQRU, 0 . 0 , SKSEED, AQRERR) 
RACQ  -  ACQRM  +  AQRERR 


C 

C 

C 

C 

C 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
•LOOP*  START 


CALL 

RECEIVE 

REAL  32BIT ( 

MAGRTR  ) 

CALL 

RECEIVE" 

'REAL  32BIT  ( 

LAMSEK  (I ) 

CALL 

RECEIVE* 

_REAL_323IT  ( 

LAMSEK (2) 

CALL 

RECEIVE 

REAL  323IT ( 

TI2MC)  ) 

CALL 

RECEIVE* 

'REAL  323IT ( 

TI2MI2)  ) 

CALL 

RECEIVE* 

REAL  3231 T ( 

TI2M ( 3 )  ) 

CALL 

RECEIVF' 

REAL  3?R'T< 

TI2MI4)  ) 

o  u  o  o  u  u 
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CALL  R£CEIVE_R£AL_32BIT (  TI2M(5)  ) 
CALL  RECEIVE_REAL_32BIT (  TI2M(6)  ) 

CALL  RECEIVE_REAL_32BIT (  TI2M<7)  ) 
CALL  REC£IVE_REAL_32BIT (  TI2M(8)  ) 
CALL  R£CEIVE_REAL_32BIT (  TI2M(9)  ) 
*  may  want  to  look  at  reordering  these 

CALL  RECEIVE_REAL_32BIT (  RREL(l)  ) 
CALL  RECEIVE_REAL_32BIT (  RREL ( 2 )  ) 
CALL  R£CEIVE_REAL_32BIT (  RREL (3)  ) 
CALL  RECEIVE_REAL_32BIT (  VREL(l)  ) 
CALL  RECEIVE_REAL  32BIT (  VREL<2)  ) 
CALL  RECEIVE_REAL~32BIT (  VREL(3)  ) 

CALL  RECEIVE  SIGNED  16BIT<  ACQD  > 


- c 

SEEKER  MODULE - C 

- c 

Calculates  LOS  angles  measured  by  the  C 

seeker  C 

C 

- c 


IF  (  TSTEP  .GE.  TSPUDRIV  )  THEN 
TSPUDRIV  =  TSPUDRIV  +  TSPUSTEP 

CALL  SEEKER (T, ACQD, LAMSEK, MAGRTR, SKSEED, FRMRAT, FRMCNT, 
SAMRAT, TRACK, TERM, SNR, LAMM) 

ENDIF 

CALL  SEND_REAL_32BIT (  FRMRAT  ) 

IF  (  TSTEP  .GE,  TSPUDRIV  )  THEN 
TSPUDRIV  =  TSPUDRIV  +  TSPUSTEP 
EMULATE  SIGNAL  PROCESSING  LAG 
LATCH  =  1 

CALL  SSPLAG (T, LAMM, RREL, VREL, TI2M,  SNR, LATCH, KFSF, TKFU, 
LAMMO, RRELO, VRELO, TI2M0, SNRO) 

ENDIF 

IF  (  TSTEP  .GE.  TKFUDRIV  )  THEN 
TKFUDRIV  *  TKFUDRIV  +  TKFUSTEP 
GET  FILTER  INPUTS  APPRORI ATE  FOR  THIS  PASS 
LATCH  =  -1 

CALL  SSPLAG (T, LAMM, RREL, VREL, TI2M, SNR, LATCH, KFSF,  TKFU, 
LAMMO, RRELO, VRELO, TI2MO, SNRO) 

ENDIF 

CALL  SEND_REAi._32BIT  (  LAMMO  (1)  ) 

CALL  SEND_REAL_32BIT (  LAMMO (2)  ) 

CALL  SEND_REAL_32BIT (  RRELO (1)  ) 

CALL  SEND_REAL_32BIT (  RRELO (2)  ) 

CALL  SEND_REAL_32BIT (  RRELO (3)  ) 

CALL  SEND_REAL_32BIT (  SNRO  ) 

CALL  SEND_REAL_32BIT (  TI2MO(l)  ) 

CALL  SEND  REAL_32BIT (  TI2MOI2)  ) 

CALL  SEND~REAL_32BIT (  TI2MO<3)  ) 

CALL  SEND_REAL_32BIT (  TI2MOI4)  ) 

CALL  SEND_REAL_32BIT (  TI2MO(5)  ) 

CALL  SEND_REAL_32BIT (  TI2MO<6)  ) 

CALL  SEND_REAL_32BIT (  TI2MO(7)  ) 

CALL  SEND_REAL_32BIT (  TI2MOI8)  ) 

CALL  SEND_REAL_32BIT (  TI2MOI9)  ) 

CALL  SEND_R£AL_323IT<  VRELO (1)  ) 

CALL  SEND  REAL_32BIT (  VRELO (2)  ) 

CALL  SEND  REAL  32BITI  VRELO (3)  ) 


TERMINATION  LOGIC 


non 


noon 
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c 

c 

♦LOOP 

♦LOOP 

FILE: 


Defines  the  simulation  termination 
conditions 


increment  time 

TSTEP  =  TSTEP  +  1.0 
T  =  TSTEP  *  DELT 

CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED_16BIT (  IEXIT  > 

STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
EPILOGUE 
END 


uuv22 . 1 9g/debug/uublXl 7 .for 


PROGRAM  BLK17 

IMPLICIT  REAL 
IMPLICIT  REAL 

INTEGER  ACQD 

REAL 

ADISTT ( A ,  3) 

REAL 

DELT 

REAL 

DLV ( 3 ) 

REAL 

DTCVU 

REAL 

DTEPS 

REAL 

DTMP1 

REAL 

DTSPVC 

INTEGER  ESTATE 
INTEGER  FLIP 
INTEGER  IBURND 
INTEGER  IBURNM 
INTEGER  ICMU 
INTEGER  IDIST 
INTEGER  1DMEAS 
INTEGER  IDPASS 
INTEGER  IDROP 
INTEGER  IEXIT 
INTEGER  IMCEND 
INTEGER  IVCS 

REAL 

MAGR 

REAL 

MAGV 

REAL 

MASS 

INTEGER  MIDBRN 

REAL 

MVR 

REAL 

MVS 

REAL 

PITER0 

REAL 

RMIR  (3) 

REAL 

ROLLER 

INTEGER  SEKTYP 

REAL 

SNRACQ 

REAL 

SNRO 

REAL 

SP 

REAL 

SQ 

REAL 

SR 

REAL 

T 

REAL 

TAPUDRIV 

REAL 

TAPUSTEP 

REAL 

TCORV 

REAu 

TDROP 

REAL 

TFFE 

REAL 

TGPUDRIV 

REAL 

TGPUSTF.P 

REAL 

TI2MI9) 

REAL 

TKFUDRI V 

REAL 

TKFUSTEP 

REAL 

TKVON 

REAL 

TMGUID 

INTEGER*  4  TOSEED 

REAL 

TSTEP 

noon 
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REAL  TSTG2 
REAL  TTF 
REAL  TTFE 
REAL  URREL  (3) 

REAL  UVS (3) 

REAL  VC (3) 

REAL  VG(3) 

REAL  VGM ( 3 ) 

REAL  VMIR_(3) 

REAL  VS  (3) 

REAL  VTT  (3) 

REAL  VTTIC ( 3 ) 

REAL  VTTP (3) 

REAL  YAWERO 

SINCLUDE ( '  'VINCLUDE/SSBLK17 . DAT • ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 

C  INITIALIZE 

VTTP (1)  -  VTTIC (1) 

VTTP  (2)  =  VTTIC (2) 

VTTP (3)  =  VTTIC (3) 


- c 

MAIN  EXECUTION  LOOP  - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
♦LOOP*  START 

CALL  RECEIVE  REAL  32BIT (  MASS  ) 


CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE_REAL_J2BIT< 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE~REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECE I VE_REAL_3  2  BIT ( 
CALL  RECEIVE_REAL_3?BIT ( 
CALL  RECEIVE_REAL_32BIT < 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE~REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE  REAL_32BIT ( 
CALL  RECEIVE~REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL_32BIT( 
CALL  RECEIVE_REAL_32BIT ( 
CALL  RECEIVE_REAL  32BIT ( 
CALL  RECEIVE  REAL' 32BIT ( 


RMIR_ ( 1 )  ) 
RMIR_ (2)  ) 
RMIR_ ( 3 )  ) 
VMIR_ ( 1 )  ) 
VMIR_(2)  ) 
VMIR_  (3)  ) 
SP  ) 

SQ  ) 

SR  ) 

TI2M ( 1 )  ) 
TI2M (2)  ) 
TI2M (3)  ) 
TI2M14)  ) 
TI2M (5)  ) 
TI2M ( 6 )  ) 
TI2M (7)  ) 
TI2M (8)  ) 
TI2M ( 9)  ) 
MVR  ) 

VTT  ( 1 )  ) 
VTT  (2)  ) 
VTT  (3)  ) 


c - C 

c - CORRELATED  VELOCITY  MODULE - C 

c - c 

C  This  section  calculates  the  correlated  C 

C  velocity  vector  (VC)  through  an  iter-  C 

C  ative  process.  From  VC,  the  steering  C 

C  velocity  vector  is  produced  by  sub-  C 

C  tracting  a  bias  velocity  (VDO)  from  the  C 

C  velocity  to  be  gained  (VG) .  C 

C  C 

C - C 


IF  (  TSTEP  .GE.  TGPUDRIV  )  THEN 


nonnnnn 
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TGPUDRIV  =  TGPUDRIV  +  TGPUSTEP 

IF  (  T.GE.TCORV  .AND.  T.LE. (TTF-DTSPVC)  )  THEN 

CALL  CORVEL (T, MVR, VTT, RMIR_, VMIR_, VTTP, VG, VS, MVS, UVS, VC, 
DLV, TFFE, TTFE) 

DTMP1  =  DTCVU  *  ANINT  (  (T+DTCVU)  /  DTCVU  ) 

TCORV  =  DTMP1 
ENDIF 

ENDIF 


CALL  R£CEIVE_REAL_32BIT (  URREL(l) 
CALL  RECEIVE_REAL_32BIT (  URREL (2) 
CALL  RECE I VE_REAL  32BIT (  URREL (3) 
CALL  RECEIVE_REAL~32BIT (  MAGR  ) 
CALL  RECEIVE_REAL_32BIT (  MAGV  ) 
CALL  RECEIVE_REAL_32BIT(  PITERO  ) 
CALL  RECEIVE_REAL~32BIT (  YAWERO  ) 


) 

) 

) 


- c 

MIDCOURSE  GUIDANCE  MODULE  - C 

- c 

Calculates  roll  error,  controls  C 

midcourse  sequencing,  and  issues  C 

midcourse  diverts  c 

- Q 


IF  (  TSTEP  .GE.  TGPUDRIV  )  THEN 
TGPUDRIV  =  TGPUDRIV  +  TGPUSTEP 
IF  (  T.GT.TSTG2  .AND. 

*  T.GE.TMGUID  .AND.  ACQD.EQ.O  :  THEN 

C  NOSE  FAIRING  /  BOOST  ADAPTER  SEPARATION 

IF  (  I DROP . EQ . 1  .OR.  (ABS ( T-TDROP ) . LE . DTEPS )  )  THEN 
I  DROP  =  2 
ENDIF 

CALL  MCGUID (T, TI2M, VG, URREL, MASS_, IDIST, MIDBRN, MAGR, 

MAGV, SP, SQ, SR, PITERO, YAWERO, FLIP, IVCS, ICMD, IDMEAS, IDPASS, 
IDROP, IMCEND, IBURND, IBURNM, VGM, ADISTT, ROLLER, 
TMGUID ) 


ENDIF 

ENDIF 


*  seeker 

CALL  RECEIVE_SIGNED  16BIT(  ACQD  ) 


CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 


SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 

SEND 


SIGNED_1 6BIT 
"SIGNED_16BIT 
"SIGNED_16BIT 
'SIGNED_16BIT 
"REAL_32BIT  ( 
"REAL_32BIT  t 
"REAL_32BIT ( 
"REAL_32BIT  ( 
"REAL_32BIT  < 
"REAL_32BIT  ( 
~REAL_32BIT  ( 
~REAL_32BIT ( 
~REAL_32BIT ( 
~REAL_32BIT  ( 
"REAL_32BIT ( 
"REAL_32BIT( 
"REAL_32BIT  ( 
"REAL_32BIT  ( 
"REAL  32BIT  ( 


(  IDROP  ) 

(  IBURND  ) 

(  IBURNM  ) 

(  IDMEAS  ) 
ADISTT (1, 1 
ADISTT (1, 2 
ADISTT (1,3 
ADISTT  (2, 1 
ADISTT (2, 2 
ADISTT (2, 3 
ADISTT (3, 1 
ADISTT  (3, 2 
ADISTT (3, 3 
ADISTT (4, 1 
ADISTT (4,2 
ADISTT  (4,  3 
VGM ( 1 )  ) 

VGM  (2)  ) 

VGM ( 3 )  ) 


) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 

) 


CALL  RECEIVE_REAL_32BIT (  SNRO  ) 
CALL  S£ND_SIGNED_1 6BIT (  IVCS  ) 
CALL  SEND_REAL_32BIT (  UVS ( 1 )  ) 
CALL  SEND_REAL_32BIT (  UVS (2)  ) 
CALL  SEND_REAL_32BIT (  UVS (3)  ) 
CALL  SEND  REAL  32BIT(  MVS  ) 


o  o  o  o  o  o  o 
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CALL  RECEIVE_SIGNED_1 6BIT (  ESTATE  ) 

IF  (  TSTEP  .GE.  TKFUDRIV  )  THEN 

TKFUDRIV  =  TKFUDRIV  +  TKFUSTEP 

C  CALL  FILTER  IF  SNR  IS  SUFFICIENT 

IF  (  SNRO.GE.SNRACQ  .OR.  SEKTYP.NE.2  )  THEN 

IF  (  ESTATE. EQ. 0  )  THEN 
ROLLER  =0.0 
ENDIF 

ENDIF 

ENDIF 

CALL  SEND_REAL_32BIT<  ROLLER  ) 

CALL  RECEIVE_SIGNED_16BIT (  MIDBRN  ) 

CALL  RECEIVE_SIGNED_16BIT (  ICMD  ) 

CALL  RECEIVE_SIGNED_16BIT<  IDIST  ) 

IF  (  TSTEP. GE.TAPUDRIV  )  THEN 

TAPUDRIV  =  TAPUDRIV  +  TAPUSTEP 

IF  (  T.GE.TKVON  )  THEN 

IF  (  IBURNM  .EQ.  0  )  THEN 

*  The  IBURNM  assignment  was  moved  from  the  partition  with  VCSLOG 
IBURNM  =  1 
ENDIF 

ENDIF 

ENDIF 


TERMINATION  LOGIC 


Defines  the  simulation  termination 
conditions 


C  increment  time 

TSTEP  =  TSTEP  +1.0 
T  =  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED  16BIT<  IEXIT  ) 

‘LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
‘LOOP*  EPILOGUE 
END 


FILE :  uuv22 . 19g/debug/uublkl8 .for 


PROGRAM  BLK18 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  ATHRF ( 4 ) 

REAL  CG(3) 

REAL  DELT 
REAL  DTOFFI4) 

REAL  FOFF1 (4) 

REAL  FOFF2 ( 4  > 

REAL  FRCX 
REAL  FRCY 
REAL  FRCZ 
INTEGER  IEXIT 
INTEGER  IFTAB 
REAL  KM 


n  n  n  n  o  o  n 
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REAL  KN 

INTEGER  LENF ( 4 ) 

REAL  MACH 
REAL  MDOTF 
REAL  MRCX 
REAL  MRCY 
REAL  MRCZ 
REAL  QA 
REAL  T 
REAL  TBRK 
REAL  TFRCS 
REAL  TFTAB 
REAL  THF (8,  4) 

REAL  TMF ( 8 ,  4) 

INTEGER* 4  TOSEED 
REAL  TSTEP 
REAL  TSTG2 
REAL  VCMD ( 4 ) 

REAL  VCMDL ( 4 ) 

$ INCLUDE ( 1 */ INCLUDE /SSBLK1 8 . DAT  1 ) 

•LOOP*  PROLOGUE 

*  INITIALIZE  80x87 
CALL  CW87 

C  INITIALIZE  UNIFORM  RANDOM  NUMBER  GENERATOR 

CALL  RANIT  (  TOSEED  ) 


*  initialization  for  purpose  of  delaying  receipt  of  actual  values 
QA  -  0.0 

MACH  =0.0 


- c 

MAIN  EXECUTION  LOOP - C 

- c 

Execution  of  all  events  is  performed  C 
within  this  loop  C 

C 

- c 


1000  CONTINUE 
LOOP*  START 


from  MASSPR 

CALL  RECEIVE_REAL_32BIT (  CG(1)  ) 
CALL  RECEIVE_REAL_32BIT (  CG(2>  ) 
CALL  RECEIVE  REAL_32BIT (  CG(3)  ) 


FRACS  THRUSTER  RESPONSE  MODULE 


Models  forces  and  moments  generated  by 
the  forward  reaction  control  system 


■C 

•C 

•C 

C 

C 

C 

•C 


IF  (  T.GE. TFRCS  .AND.  T.LE.TSTG2  )  THEN 

CALL  FRCTHR (T, CG, MACH, QA, VCMD, VCMDL, DTOFF, TFTAB, IFTAB, 

TOSEED, TBRK, TMF, THF, LENF, FRCX, FRCY, FRCZ, MRCX, 
MRCY, MRCZ , MDOTF, ATHRF, KN , KM, FOFF1 , FOFF2 ) 

ENDIF 


CALL  SEND_REAL_32BIT < 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_RrAL_32BIT ( 
CALL  SEND_RLAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND_REAL_32BIT ( 
CALL  SEND  REAL  32BIT { 


FRCX  ) 
FRCY  ) 
FRCZ  ) 
MRCX  ) 
MRCY  ) 
MRCZ  ) 
MDOTF  ) 
FOFF1  (1) 
FOFF1  (2) 
FOFF1 (3) 
FOFF1 (4) 
FOFF2 (1) 
F0FF2 (2) 
FOFF2 (3) 


) 

) 

! 

) 

) 

) 

) 
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CALL  SEND  REAL  32BIT  (  FOFF2(4)  ) 


*  from  AERO  (delayed) 

CALL  RECEIVE_REAL_32BIT (  MACH  ) 
CALL  RECEIVE  REAL  32BIT (  QA  ) 


*  fracs 

CALL 

CALL 

CALL 

CALL 

CALL 

CALL 


RECEIVE_REAL_32BIT (  VCMD(l)  ) 
RECEIVE  REAL_32BIT (  VCMD(2)  ) 
R£C£IVE~REAL_32BIT (  VCMD(3)  ) 
RECEIVE_REAL_32BIT (  VCMD ( 4 )  ) 
RECEIVE_SIGNED_16BIT <  IFTAB  ) 
RECEIVE  REAL  32BIT (  TFTAB  ) 


TERMINATION  LOGIC 


Defines  the  simulation  termination 
conditions 


C  increment  time 

TSTEP  *  TSTEP  +1.0 
T  -  TSTEP  *  DELT 

C  CONTINUE  LOOPING  UNTIL  ONE  OR  MORE  EXIT  CONDITIONS  HAVE  BEEN  MET 

CALL  RECEIVE_SIGNED_16BIT<  IEXIT  ) 

♦LOOP*  STOP 

IF  (  IEXIT. EQ.O  )  GO  TO  1000 
*  LOOP  *  EPILOGUE 
END 


FILE:  uuv22 . 19g/debug/uuexosim. txt 


#  tfinal 
real  32bit 
1 

145.0 


FILE :  uuv22 .19g/dutility/ make file 


FORFLAGS  =  code  large  optimize (3)  storage (integer*2) 


OBJECTS  =  \ 

SSKVAUTO.OBJ  \ 
SSVCSLOG . OBJ  \ 
UU ACCEL. OBJ  \ 
UUCW87 . OBJ  \ 
UUESTREL.OBJ  \ 
UUFV2BXI .OBJ  \ 
UUIMUPRO.OBJ  \ 
UUINTEG . OBJ  \ 
UUINTEGI .OBJ  \ 
UUKALMAN . OBJ  \ 
UUMASSPR . OBJ  \ 
UUMCAUTO . OBJ  \ 
"UMISSLT . OBJ  \ 
UUMMK . OBJ  \ 
UUMMLXY . OBJ  \ 
UUNAVIG . OBJ  \ 
UUNORM.OBJ  \ 
UUOBTARG.OBJ  \ 
UUOUTMES .  OBJ  \ 
UURAN . OBJ  \ 
UURAN0 . OBJ  \ 
UURAN IT . OBJ  \ 
UURELAT . OBJ  \ 
UURESP2R . OBJ  \ 


o  o  o  n  n  o  n 
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UU RE ST HR . OBJ  \ 
UUROTMX . OBJ  \ 
UUTABLE . OBJ  \ 
UUTARGET . OBJ  \ 
UUVCSTH2 . OBJ  \ 
UUTIMER . OBJ 


LIBRARY  =  UTILITY. LIB 


$ (LIBRARY) : S (OBJECTS) 


.  for . ob j : 

ftn286.new  $<  $(forflags) 

bnd286  $*.obj  name(S’)  object  (S* .  IniO  noload  noprint 

rename  $*.lnk  over  S'.obj 

submit  :PFP : csd/lib (  S (LIBRARY),  $*  ) 


clean: 

delete  *. obj 1st , S (LIBRARY) 
FILE:  uuv22 . 1 9g/dutility/uuaccel . 


for 


SUBROUTINE  ACCEL (T, UD, VD 
GYSEED, 

,  WD,  P,  Q,  R,  PD,  QD 
QFRACA, PULSEA) 

, RD, CG, CIM, XD, YD, ZD, GR, 

c 

c 

SUBROUTINE  NAME  : 

ACCEL 

c 

AUTHOR (S)  : 

D. 

C.  FOREMAN 

c 

FUNCTION  : 

ACCELEROMETER  MODEL 

,  COMPUTES  SENSED  DELTA 

c 

VELOCITY  COUNTS. 

INCLUDES  ROTATIONAL 

c 

EFFECTS,  AXIS  MIS 

ALIGNMENT  AND  NONORTHOGON- 

c 

ALITY  ERRORS,  SCALE 

:  FACTOR  ERRORS,  RANDOM 

c 

AND  CONSTANT  DRIFT 

AND  QUANTIZATION. 

c 

CALLED  FROM  : 

FORTRAN  MAIN 

c 

SUBROUTINES  CALLED 

:  NORM  ,  RESP2R 

c 

INPUTS  : 

T, 

UD,  VD, WD , P , Q, R, 

PD,  QD,  RD,  CG,  CIM,  XD, 

c 

YD 

,  ZD, GR 

c 

OUTPUTS  : 

NONE 

c 

BOTH  : 

GYSEED, QFRACA,  PULSEA 

c 

UPDATES  : 

T. 

THORNTON  -  CR 

# 

004 

c 

T. 

THORNTON  -  CR 

* 

016 

c 

B. 

HILL  -  CR 

» 

020 

c 

D. 

SMITH  -  CR 

* 

021 

c 

B. 

HILL  -  CR 

* 

022 

c 

B. 

HILL  -  CR 

030 

c 

T. 

THORNTON  -  CR 

# 

037 

c 

B. 

HILL  -  CR 

# 

038 

c 

D. 

SMITH  -  CR 

* 

059 

c 

D. 

SISSOM  -  CR 

# 

069 

c 

D. 

SMITH  -  CR 

4 

070 

c 

D. 

SMITH  -  CR 

4 

075 

c 

D. 

SMITH  -  CR 

# 

076 

c 

B. 

HILL  /  -  CR 

# 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE  -  CR 

# 

084 

c 

R. 

RHYNE  -  CR 

# 

087 

c 

c 

c--- 

B. 

HILL  -  CR 

* 

093 

IMPLICI 

T  DOUBLE  PRECISION 

(A-H) 

IMPLICI 

T  DOUBLE  PRECISION 

(C-Z) 

DOUBLE 

PRECISION  A3I 0(3) 

,  A3I 1(3) 

,  ABI  2(3) 

DOUBLE 

PRECISION  A3O0 ( 3 ) 

,  A301 ( 3 ) 

,  AB02I3) 

REAL 

cg  ( : ) 

,  CIM  (9) 
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DOUBLE 

PRECISION 

DCAI3) 

DOUBLE 

PRECISION 

DUM1 (3) 

,  DUM2(3) 

,  DUM3 ( 3 ) 

DOUBLE 

PRECISION 

DVEL  (3) 

,  GRAVG  (3) 

DOUBLE 

PRECISION 

GR  ( 3 ) 

DOUBLE 

PRECISION 

GRLST  (3) 

,  LIMU ( 3 ) 

,  PULSEA ( 3 ) 

DOUBLE 

PRECISION 

QFRACA (3) 

,  SF1A  ( 3 ) 

,  SF2A ( 3 ) 

DOUBLE 

PRECISION 

SFEA ( 3 ) 

,  WDRA ( 3 ) 

DOUBLE 

PRECISION 

XIMU (3) 

,  XYZDP (3) 

REAL  P,  Q,  R,  PD,  QD,  RD 
INTEGER*  4  GYSEED 


C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IACCEL 

*  DATA  INITIALIZATION 

$ INCLUDE  C  VINCLUDE/SSACCEL-DAT' ) 

^INCLUDE ( 1  'VINCLUDE/SSCON1S.DAT1 ) 

SINCLUDE ( ’ VINCLUDE/SSC0N16.DAT' ) 

DATA  IACCEL  /  1  / 

IF  (IACCEL  .EQ.  1)  THEN 
IACCEL  =  0 

C  INITIALIZE  ACCELEROMETER  PARAMETERS 

IF  (  T  .EQ.  0.0  )  THEN 

DRSIGA  =  DRSGAI/ (60 . 0*DSQRT (DTIMU) ) 

CALL  NORM (ALNSGA, ALNMNA, GYSEED, PSIA) 

CALL  NORM (ALNSGA, ALNMNA, GYSEED, THTA) 

CALL  NORM (ALNSGA, ALNMNA, GYSEED, PHIA) 

CALL  NORM (AORSGA, AORMNA, GYSEED, THXZA) 

CALL  NORM (AORSGA, AORMNA, GYSEED, THXYA) 

CALL  NORM (AORSGA, AORMNA, GYSEED, THYZA) 

CALL  NORM (AORSGA, AORMNA, GYSEED, THYXA) 

CALL  NORM (AORSGA, AORMNA, GYSEED, THZYA) 

CALL  NORM (AORSGA, AORMNA, GYSEED, THZXA) 

CALL  NORM (SF1SGA, SF1MNA, GYSEED, SF1A(1> ) 

CALL  NORM (SF1SGA, SF1MNA, GYSEED, SF1A (2) ) 

CALL  NORM (SF1SGA, SF1MNA, GYSEED, SF1A(3> ) 

CALL  NORM (SF2SGA, SF2MNA, GYSEED, SF2A (1 ) ) 

CALL  NORM (SF2SGA, SF2MNA, GYSEED, SF2A (2 ) ) 

CALL  NORM (SF2SGA, SF2MNA, GYSEED, SF2A ( 3) ) 

CALL  NORM (DCSIGA, DCMENA, GYSEED, DCA(l) ) 

CALL  NORM(DCSIGA, DCMENA, GYSEED, DCA12) ) 

CALL  NORM(DCSIGA, DCMENA, GYSEED, DCA(3) ) 

DO  10  I  =  1,3 

ABI2 (I)  =  0.0D0 
ABI 1 (I)  =  0.0D0 
ABO 2 (I)  =  O.ODO 
ABOl (I)  =  O.ODO 
10  CONTINUE 

ENDIF 

C  COMPUTE  SECOND  ORDER  RESPONSE  DIFFERENCE  EQUATION  COEFFICIENTS 

IF  (  IARTYP . EQ . 2  )  THEN 

CALL  RESP2R  (  DTIMU, WACC, ZACC, CABI 2, CABI 1 , CABI 0 , CAB02 , 
CABOl , CABOO  ) 

ENDIF 

ENDIF 

C  CALCULATE  TIME  SINCE  LAST  CALL  TO  ACCEL 

DTDEL  =  T  -  T0ACCE 
T0ACCE  =  T 

C  DETERMINE  INERTIAL  FRAME  DELTA  VELOCITY  OVER  PREVIOUS  INTERVAL  W 

C  GRAVITIONAL  CONTRIBUTION  REMOVED 

IF  (  DTDEL. NE. O.ODO  )  THEN 

GRAVG(l)  =  0.5D0  *  (GR(1)  +  GRLST(l)  ) 

GRAVG ( 2 )  =  O.5D0  *  (  GR ( 2 )  *  GRLST ( 2 )  ) 

GRAVG ( 3 )  =  0.5D0  •  (  GR(3)  +  GRLST ( 3 )  ) 

DLVXI  =  XD  -  XYZDP(l)  -  DTDEL'GRAVG ( 1 ) 

DLVYI  =  YD  -  XYZDP  (  2  )  -  DTDEL* GRAVG  ( 2  ) 

DLVZI  =  ZD  -  XYZDP (3)  -  DTDEL* GRAVG ( 3 ) 
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ENDIF 

C  SAVE  GRAVITY  VECTOR  FOR  USE  ON  NEXT  PASS 

GRLST(l)  =  GRID 
GRLST ( 2 )  =  GR(2) 

GRLST ( 3 )  =  GR ( 3 ) 

C  ROTATE  DELTA  VELOCITY  INTO  MISSILE  FRAME 

IF  (  DTDEL.NE.0.0D0  )  THEN 

DLVXB  =  CIM<1) 'DLVXI  +  CIM(4)'DLVYI  +  CIM<7)*DLVZI 

DLVYB  =  CIM12) 'DLVXI  +  CIM(5)*DLVYI  +  CIM(8)*DLVZI 

DLVZB  =  CIM(3) 'DLVXI  -r  CIM(6)*DLVYI  +  CIM(9)'DLVZI 

ENDIF 

C  CONVERT  DELTA  VELOCITY  TO  AVERAGE  ACCELERATION 

IF  (  DTDEL.NE.0.0D0  )  THEN 
UDAVG  =  DLVXB  /  DTDEL 

VDAVG  -  DLVYB  /  DTDEL 

wDAVG  =  DLVZB  /  DTDEL 

ELSE 

UDAVG  =  UD 

VDAVG  -  VD 

WDAVG  =  WD 

ENDIF 

C  SAVE  PREVIOUS  INERTIAL  FRAME  VELOCITY 

XYZDP(l)  »  XD 
XYZDP (2)  =  YD 
XYZDP ( 3 )  =  ZD 

C  SENSOR  ACCELERATION  DUE  TO  PACKAGE  OFFSET  FROM  THE  CG 

IF  (  IMUOFF.EQ.O  )  THEN 
U  DR  =  UDAVG 

VDR  =  VDAVG 

WDR  =  WDAVG 

ELSE 

XIMU(l)  =  CG  ( 1 )  -  LIMU(l) 

XIMU ( 2 )  «  CG (2)  -  LIMU (2) 

XIMU (3)  =  CG  ( 3)  -  LIMU  (3) 

DUMl(l)  »  QD'XIMU ( 3 )  -  RD*XIMU(2) 

DUM112)  =  RD'XIMU ( 1 )  -  PD'XIMU ( 3 ) 

DUM1 (3)  =  PD'XIMU (2)  -  QD'XIMU(l) 

DUM2 ( 1 )  =  Q*XIMU<3)  -  R*XIMU(2) 

DUM2 (2)  =  R'XIMU(l)  -  P'XIMU(3) 

DUM2 ( 3 )  =  P 'XIMU (2 )  -  Q'XIMU(l) 

DUM3 { 1 )  =  Q'DUM2<3)  -  R*DUM2<2) 

DUM3 (2)  =  R' DUM2 ( 1 )  -  P*DUM2(3) 

DUM3 ( 3 )  =  P'DUM2(2)  -  Q'DUM2(1) 

UDR  =  UDAVG  +  DUMl(l)  +  DUM3<1> 

VDR  =  VDAVG  +  DUM1 (2)  +  DUM3I2) 

WDR  =  WDAVG  +  DUM1 ( 3 )  +  DUM3<3) 

ENDIF 

C  ACCELEROMETER  AXIS  MISALIGNMENT  EFFECTS 

UDM  =  UDR  +  VDR'PSI A  -  WDR'THTA 

VDM  =  -  UDR'PSIA  +  VDR  +  WDR'PHIA 

WDM  =  UDR'THTA  -  VDR'PHIA  ♦  WDR 

C  ACCELEROMETER  AXIS  NONORTHOGCNALITY  EFFECTS 

UDN  »  UDM  *  VDM'THXZA  -  WDM'THXYA 

VDN  -  -  UDM'THYZA  +  VDM  *  WDM'THYXA 

WON  UDM'THZYA  -  VDM'THZXA  ♦  WDM 

C  ADD  LINEAR  AND  QUADRATIC  SCALE  FACTOR  ERRORS 

SFEA(l)  =  UDN  *  SFIAO'UDN  -  SF2A  ( 1 )  *UDN"2 

SFEA  ( 2 )  =  VDN  *■  SFIA(2)'VDN  *  SF2AI2)  *VDN"2 

SFEA  !  3 )  =  WDN  +  SF1A(3)'WDN  -  SF2A  (  3 )  *WDN"2 


FOR  EACH  AXIS  . . . 
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DO  20  1=1, 3 

C  MAKE  A  GAUSSIAN  DRAW  FOR  RANDOM  DRIFT  AND  ADD  TO  CONSTANT  DRIFT 

IF  (  DRSIGA.GT.O.ODO  )  THEN 

CALL  NORM (DRSIGA, DRMENA, GYSEED, DRA) 

ENDIF 

WDRA(I)  =  DRA  +  DC  A  ( I ) 

C  COMPUTE  INPUT  TO  ACCELEROMETER  RESPONSE  MODEL 

ABIO(I)  =  SFEA(I)  +  WDRA(I) 

C  SECOND  ORDER  RESPONSE  MODEL 

IF  (  IARTYP.EQ.2  )  THEN 

ABOO(I)  =  (  CABIO’ABIOd)  +  CABIl’ABIl (I) 

+  CABI2*ABI2 (I)  -  CABOl “ABOl ( I ) 

-  CAB02*AB02(I)  ) /CABOO 
A. 3 1 2  (I)  =  ABI 1  (I) 

ABI1  (I)  =  ABI 0(1) 

AB02  (I)  -  ABOl  (I) 

ABOl (I)  =  ABOO (I) 

ENDIF 

C  INSTANTANEOUS  RESPONSE  MODEL 

IF  (  IARTYP.EQ.O  )  THEN 
ABOO (I)  =  ABIO (I) 

ENDIF 

C  COMPUTE  SENSED  DELTA  VELOCITi 

DVEL(I)  =  DTDEL  *  ABOO (I) 

IF  (  SPPA.GT.0.0  )  THEN 
C  UNQUANTIZED  OUTPUT  IN  COUNTS 

QFRACA(X)  =  QFRACA(I)  -  PULSEAd)  +  DVE’,(I)/SPPA 
C  QUANTIZED  OUTPUT  IN  COUNTS 

PULSEAd)  =  DINT  (QFRACA  (I )  ' 

ELSE 

PULSEAd)  =  DVELd) 

ENDIF 

20  CONTINUE 

RETURN 

END 


FILE:  uuv22 . 19q/dutility/uucw87. for 


subroutine  cw87 

inteqer*2  icw87 

call  stcw87 ( icw87 ) 

icw87  ■=  icw87  .and.  #ff7ah 

call  ldcw87 ( icw87) 

end 


FILE :  uuv22 . 1 9q/dut i 1 i ty /uuest rel .for 


C -  - 

SUBROUTINE  ESTREL 'RTEST, VTEST, RMIR, VMIR, TI2M,  CMS, ESTATE, RREL, 
'REL,  MAGR,  MACV,  URREL,  MGRDOT,  TGO,  PITER,  YAWER, 
Lu.MD) 


C 

C  SUBROUTINE  NAME  :  ESTREL 

C  AUTHOR (S)  :  T.  THOUNTON 

C 
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C  FUNCTION  : 


COMPUTES  ESTIMATED  RELATIVE  RANGE,  RANGE 
RATE,  AND  TIME-TO-GO 


CALLED  FROM  : 
SUBROUTINES  CALLED 
INPUTS  : 

OUTPUTS  : 


FORTRAN  MAIN 


NONE 


RTEST, VTEST, PMIR, VMIR, TI2M, CMS, ESTATE 

RREL, VREL, MAGR, MAGV, URREL, MGRDOT,  TGO, 
PITER, YAWER, LAMD 


c 

UPDATES  : 

D. 

SMITH 

-  CR 

* 

059 

c 

R. 

RHYNE 

-  CR 

* 

068 

c 

D. 

SISSOM 

-  CR 

# 

C69 

c 

E. 

HILL  / 

-  CR 

H 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

-  CR 

# 

C88 

c 

R. 

RHYNE 

-  CR 

n 

093 

IMPLICIT  DOUBLE  PRECISION  (A-H) 
IMPLICIT  DOUBLE  PRECISION  (O-Z) 


DOUBLE 

PRECISION 

CMS  (9) 

LAMD (2) 

,  LAMSKE ( 2 ) 

REAL 

MAGR 

MAGV 

,  VRDRR 

REAL 

PITER 

YAWER 

,  TGO 

DOUBLE 

PRECISION 

MGRDOT 

DOUBLE 

PRECISION 

RELM ( 3 ) 

RELS ( 3 ) 

,  RMIR  (  3 ) 

DOUBLE 

PRECISION 

RTEST (3) 

REAL 

TI2M ( 9) 

REAL 

URREL  (3) 

VREL { 3 ) 

,  RREI (3) 

DOUBLE 

PRECISION 

VELM ( 3 ) 

VELS (3) 

DOUBLE 

PRECISION 

VMIR ( 3 ) 

VTEST  (3) 

INTEGER 

ESTATE 

COMPUTE 

;  ESTIMATED 

RELATIVE  STATES 

AND  ESTIMATED 

TIME-TO-GO 

RREL ( 1 ) 

=  RTEST  (1/ 

-  RMIR(l) 

RREL  (2 ) 

=  RTEST (2) 

-  RMIR ( 2 ) 

RREL (3) 

=  RTEST (3) 

-  RMIR ( 3 ) 

MAGR  -  SQRT  (RREL  U)  **2  +  RREL(2)*'2  +  RREL(3>'*2) 

URREL (1)  =  RREL (1) /MAGR 
UAP’-.L  (2)  ~  RREL  (2) /MAGR 
URREL ( 3 )  =  RREL (3) /MAGR 

VREL(l)  =  VTEST ( i )  -  VMIR(l)  • 

VREL  12)  =  VTEST (2)  -  VMIR (2) 

VREL (3)  =  VTEST (3)  -  VMIR (3) 

MAGV  »  SQRT  (VREL  (1)  "2  t  VREL  (2)  ”2  +  VREL  (3)*  *2) 

«GRDOT  =  VREL (1) 'URREL (1)  *  VREL ( 2 ) * URREL ( 2 )  *  VREL ( 3 ) ‘URREL ( 3 > 
VRDRR  --  VREL  ( 1 )  *RREL  ( 1 )  +  VREL  ( 2  i  'RREL  ( 2  )  »  VREL  !  3 ) 'RREL  !  3 ) 
TGO  =  -VRDRR/ (MAGV**2) 

IF  (  ESTATE. EQ.i  )  THEN 

COMPUTE  ESTIMATED  RELATIVE  STATES  MISSILE  FRAME 


RELM  (1) 

=  RREL ( 1 

)  *T 1 2M ( 1 ) 

*  RREL (2) 

•t:?m  (d 

-  RREL ( 3 )  *  T 

I  2M 

RELM (?) 

=  RREL ( 1 

)  *TI2M (2) 

-  RREL  (2) 

'  T I  2  M  (  5  ) 

-  RREL(3) 'T 

'py 

RELM (3) 

RREL ( 1 

> *TI2M (3! 

*  RREL (2) 

*712X16) 

♦  RREL ( 3  > *T 

I  2M 

V ELM  (I  ) 

•■=  VREL  (1 

) 'TI2M ( I ) 

*  VREL ( 2  ) 

*7 1 2M ( 4 ) 

-  VREL (3) *7 

1 2M 

VELM (2) 

=  VREL { 1 

) *TI2M (2) 

-  VREL  (2) 

'TL2M (5) 

•  PEL ( 3) '7 

I2M 

VELM (3) 

=  VREL a 

)  *TI2M  (3) 

-  VREL  (2) 

*  T I 2M { 6 , 

*  VREL ( 3 ) ' I 

:  2  m 

COMPUTE 

ESTIMATE 

j  PELA7IV 

E  STATES  I 

N  SEEKER 

FRAME 

PELS ( 1 ) 

=  r  LM  ( 1 

) •CMS ( : ) 

*  RELM  (2  i* 

CMS (4)  - 

RELM (3)  *  CMS 

<  ') 

PELS  (2) 

=  RELM (I 

) 'CMS (2) 

►  RELM  (2)' 

CMS  (  b  l  • 

RELM l 3 i *CMS 

{  8) 

PELS (3) 

*  RELM (I 

)  *  CMS ( 3 ; 

«  RELM (2)* 

C  M  S  (  £  i  * 

RELM (  i )  -CMS 

(  9) 

VELS (I) 

=  VELM ( 1 

) 'CMS (I i 

*  VELM (2)* 

CMS  (■'.)  • 

VELM { 3 ) -CMS 

{  7  \ 

VELS (2) 

=  VELM ( 1 

) 'CMS  (2) 

*  VELM  ■  2  i  * 

:ms(D  * 

V?  ;  M  (  3)  'CMS 

(  8i 

VELS  I  3) 

-  VELM (I 

)  'CMS  (  5  i 

*  VELM (2)* 

C  M  L  (  b  ) 

VELM { 3 )  *  CMS 

(9; 
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C  COMPUTE  ESTIMATED  LINE  OF  SIGHT  ERRORS 

LAMSKE ( 1 )  =  DAT AN 2 (-RELS (3) , PELS (1) ) 

LAMSKE ( 2 )  =  DATAN2 (  RELS ( 2 ) , RELS ( I ) ) 

PITER  =  LAMSKE (1) 

YAWER  =  -LAMSKE (2) 

C  COMPUTE  ESTIMATED  LINE  OF  SIGHT  RATE  ERRORS 


LAMD ( 1 ) 
LAMD (2) 
ENDIF 


(RELS  (3)  *VELS  (1)  -  RELS ( 1 ) * VELS ( 3 ))  / 
(RELS  ( 1 )  *  *  2  *  RELS(3)"2) 

(RELS (1) *VELS (2)  -  RELS (2) 'VELS (1) )  / 

(RELS  ( 1 )  "2  +  RELS  { 2 )  " 2 ) 


RETURN 

END 


FILE :  uuv22 . 19g/dutility/uufv2bxi.for 


SUBROUTINE  FV2BXI  ( 

FV,  FVSQ,  B  ) 

c 

c 

c 

SUBROUTINE  NAME  : 

FV2BXI 

c 

C 

AUTHOR (S)  : 

W.  E.  EXELY 

C 

FUNCTION  : 

COMPUTE  DIRECTION  COSINE  MATRIX  (B) 

FROM 

C 

THE  QUATERNION  ATTITUDE  VECTOR  (FV) 

AND 

c 

C 

c 

COMPUTE  THE  SQUARE  (FVSQ)  OF  THE  MAGNITUDE 

OF  THE  QUATERNION  (FV) 

C 

c 

CALLED  FROM  : 

MISS I L 

c 

c 

SUBROUTINES  CALLED 

:  NONE 

C 

C 

INPUTS  : 

FV 

c 

C 

OUTPUTS  : 

FVSQ,  B 

c 

c 

r - 

UPDATES  : 

D.  SMITH  -  CR  #  59 

C 

IMPLICIT  REAL  ( A-H ) 
IMPLICIT  REAL  (O-Z) 


DIMENSION  FV  (  4  ) ,  B  (  9  ) 

PARAMETER  (R1  =  1.0,  R2  =  2.C) 

FVlSQ  -  FV  ( 1 )  *FV(1) 

FV2SQ  =  FV ( 2 ) *FV ( 2 ) 

FV3SQ  =  FV ( 3 )  *  F V ( 3 ) 

FV4SQ  =  FV ( 4 ) ' FV (’ ) 

FVSQ  =  FVISQ  +  FV2SQ  +  FV3SQ  +  FV4SQ 

I F (  FVSQ  .GT.  0.0  )  THEN 
*  FTN286  X415  OPTIMIZE!!) 

99999  CONTINUE 

Ti  *  R2/FVSQ 


T2 

= 

FV  O ) 

' FV ( 4 ) 

T3 

= 

fv  ( : ) 

*FV (2) 

B  ( 2 ) 

= 

Tl*  { 

T3  -  T 

B  (4) 

= 

TI  *  ( 

T3  -  T 

T2 

= 

FV  { 2 ) 

*  FV ( 4 ) 

T  3 

* 

FV  (1) 

*FV (3) 

B  ( 7 ) 

= 

Tl*  ( 

r  3  *  X 

B  ( 3 ) 

- 

TI  *  ( 

T  3  -  T, 

T2 

FV  { I ) 

*FV(4) 

T  3 

= 

FV  ( 2 ) 

*FV (3) 

3(6) 

= 

TI  *  ( 

T  3  ->■  T 

8(8) 

t:  *  < 

T3  -  T 
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T2  =  T1*FV4SQ  -  R1 
B { 1 )  =  T1*FV1SQ  +  T2 
B(5)  =  T1*FV2SQ  +  T2 
B ( 9)  =  T1*FV3SQ  +  T2 
ENDIF 

RETURN 

END 


FILE :  uuv22 . 19g/dutility/uuimupro .for 


C - 

SUBROUTINE  IMUPRO (T, PULSEG, PULSEA, DELPHI ,  DELTHT, DELPSI, DELU, 
DELV.DELW) 


C 

C  SUBROUTINE  NAME  :  IMUPRO 

C 

C  AUTHOR (S)  :  T.  THORNTON 

C 

C  FUNCTION  :  COMPUTES  THE  IMU  PROCESSOR  RELATED  FUNCTIONS 

C 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  NONE 

C 

C  INPUTS  :  T, PULSEG, PULSEA 

C 

C  OUTPUTS  :  DELPHI , DELTHT, DELPSI , DELU, DELV, DELH 

C 


c 

UPDATES  : 

T. 

THORNTON 

-  CR 

* 

004 

c 

T. 

THORNTON 

-  CR 

* 

016 

c 

B. 

HILL 

-  CR 

* 

022 

c 

T. 

THORNTON 

-  CR 

* 

037 

c 

D. 

SMITH 

-  CR 

* 

059 

c 

D. 

SMITH 

-  CR 

« 

070 

c 

D. 

SMITH 

-  CR 

it 

075 

c 

B. 

HILL  / 

-  CR 

# 

081 

c 

R. 

RHYNE 

c 

B. 

HILL 

-  CR 

it 

093 

C 

c - 

IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

DOUBLE  PRECISION  PULSEA ( 3 ) 

REAL  PULSEG (3) 

*  DATA  INITIALIZATION 
S INCLUDE ( ' V  INCLUDE /SSCON54 .DAT' ) 

C  GYRO  OUTPUT  COMPENSATION 

C  CALCULATE  DELTA  ANGLES 

IF  (  PERPG . GT .0.0  )  THEN 
DELPHS  =  PULSEG (1) *PERPG 
DELTHS  =  PULSEG (2) *PERPG 
DELPSS  »  PULSEG (3) *PERPG 
ELSE 

DELPHS  »  PULSEG (1) 

DELTHS  =  PULSEG (2! 

DELPSS  =  PULSEG (3) 

END  IF 

C  COMPENSATE  SENSED  DELTA  ANGLES  FOR  SCALE  FACTOR  ERRORS 

DELPH  =  DELPHS'SFCGX 
□  ELTH  =>  DELTHS'SFCGY 
DELPS  -  DELPSS'SFCGZ 

C  COMPENSATE  SENSED  DELTA  ANGLES  FOR  GYRO  MISALIGNMENTS 

DELPHI  -  DELPH  -  DELTH'PSIGP  -  DELPS'T.HTG? 

DELTHT  *  DELPH*PSIC?  *  DELTH  -  DELPS'PHIGP 

DELPSI  •  -DELPH'THTGP  +  DELTH-PHIGP  ♦  DELPS 
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C  ACCELEROMETER  OUTPUT  COMPENSATION 

C  CALCULATE  DELTA  VELOCITY 

IF  (  PERPA.GT.0.0  )  THEN 
DELUS  =  PULSEA (i) 'PERPA 
DELVS  =  PULSEA (2) *PERPA 
DELWS  =  PULSEA ( 3 ) ’PERPA 
ELSE 

DELUS  -  PULSEA (1) 

DELVS  «  PULSEA (2) 

DELWS  =  PULSEA (3) 

END  IF 

C  COMPENSATE  SENSED  VELOCITY  FOR  SCALE  FACTOR  ERRORS 

DELXS  =  DELUS'SFCAX 
DELYS  -  DELVS*SFCAY 
DELZS  =  DELWS*SFCAZ 

C  COMPENSATE  SENSED  VELOCITY  FOR  ACCELEROMETER  MISALIGNMENTS 

DELUM  =  DELXS  -  DELYS’PSIAP  +  DELZS'THTAP 

DELVM  =  DELXS’PSIAP  +  DELYS  -  D£LZS*PHIAP 

DELWM  -  -DELXS'THTAP  +  DELYS’PHIAP  +  DELZS 

C  SKULLING  COMPENSATION 


IF  (  ISKULL.EQ.O  ) 

THEN 

DELU 

-  DELUM 

DELV 

=  DELVM 

DELW 

=  DELWM 

ELSE 

DELU 

=  DELUM 

-  0.5  * 

(  DELPSI 'DELVM  - 

DELTHT'DELWM 

DELV 

=  DELVM 

-  0.5  * 

(  DELPHI 'DELWM  - 

DELPSI 'DELUM 

DELW 

=  DELWM 

-  0.5  * 

1  DELTHT'DELUM  - 

DELPHI'DELVM 

END  IF 


RETURN 

END 


FILE:  uuv22 .19g/dutility/uuinteg.for 


SUBROUTINE  INTEG  (  X 

,  XDOT  ,  T  ,  I  ) 

c 

c 

SUBROUTINE  NAME  : 

INTEG 

c 

c 

AUTHOR (S)  : 

D.  F.  SMITH 

c 

c 

FUNCTION  : 

Perform  simple  trapezoidal 

integration  of 

c 

XDOT  to  yield  X.  DTD  is  the  time  since 

c 

the  last  integration  and  I 

is  the  array 

c 

c 

c 

CALLED  FROM  : 

index  where  X  is  stored 

FORTRAN  MAIN 

c 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

c 

INPUTS  : 

XDOT,  T,  I 

c 

c 

OUTPUTS  : 

X 

c 

c 

UPDATES  : 

D.  SISSOM  -  CR  »  58 

c 

c 

c - 

D.  SMITH  -  CR  #  59 

C 


COMMON/STORAG/ 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 


XINT, 

XINT (50) , 
DT, 

XDOT, 


TINT, 

TINT (50) , 
DTMP, 

T 


XDOTL 
XDOTL (SO) 
X 


DT  =  T  -  TINT ( I ) 


XINT (I) 
X 


XINT  (I) 
XINT  (I) 


O.SDC'OT' (XDOT*XDOTL ( I ) i 
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TINT ( I )  =  T 

XDOTL(I)  =  XDOT 

C  TEMPORARY  CODE  TO  NORMALIZE  QUATERNION  AFTER  4TH  COMPONENT  IS  REVISED 

IF  (  I.EQ.18  )  THEN 

DTMP  =  DSQRT  (  XINT(15)**2  +  XINT<16)*»2  +  XINT(17)**2  + 

XINT(18)*»2  ) 

XINT  (15)  =  XINT  (15)  /  DTMP 
XINT  (16)  =  XINT  (16)  /  DTMP 
XINT (17)  =  XINT (17)  /  DTMP 
XINT (18)  =  XINT (18)  /  DTMP 
END  IF 

RETURN 

END 


FILE :  uuv22 . 1 9g/dut ility/ uuintegi.for 


SUBROUTINE  INTEGI  (  X  ,  XDOT  ,  T  ,  I  ) 


SUBROUTINE  NAME  :  INTEGI 

AUTHOR (S)  :  D.  F.  SMITH 

FUNCTION  :  Initialize  integral  of  X  which  is  stored 

in  position  I  of  the  integral  array 

CALLED  FROM  :  MAIN 

SUBROUTINES  CALLED  :  NONE 

INPUTS  :  X, XDOT, T, I 

OUTPUTS  :  NONE 

UPDATES  :  D.  SISSOM  -  CR  #  58 

D.  SMITH  -  CR  #  59 


COMMON/STORAG/  XINT,  TINT,  XDOTL 

DOUBLE  PRECISION  XINTI50),  TINT(SO),  XDOTLI50) 

DOUBLE  PRECISION  X,  T,  XDOT 

XINT ( I )  =  X 

XDOTL (I)  =  XDOT 
TINT ( I )  =  T 

RETURN 

END 


FILE:  uuv22 . 1 9g/dut i 1 i ty /uukalman . for 


c - 

SUBROUTINE  KALMAN (T, TI2M, LAMMO, ASIG, SNRO, TGO, RRELO, VRELO, TI2MO, 

RACQ, MAGRTR, MAGR, MAGV, LAMSEK, LAMDXX, FRMRAT, CMS, 
MACQ, MCSO, MTERM, TRACK, TERM, TRMTGO, TGE1 , 

TGE2AL, WFILT, ZFILT, LAM, LAMD, IBURN1 , ACQD, ESTATE, 
PITER, YAWER, TGI u) 

C - 

c 

C  SUBROUTINE  NAME  :  KALMAN 

C 

C  AUTHOR (S)  :  D.  F.  SMITH 

C 

C  FUNCTION  :  2-STATE  EXTENDED  KALMAN  FILTER 

C  ESTIMATES  LOS  ANGLES  AND  RATES 

C 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  NONE 

C 

C  INPUTS  :  T, TI 2M, LAMMO, ASIG,  :  JRO,  TGO,  RRELO,  VRELO, 

C  TI2MO, RACQ, MAGRTR, MAGR, MAGV, LAMSEK, LAMDXX, 


C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 
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OUTPUTS 


BOTH  : 


FRMRAT, CMS, MACQ, MCSO, MTERM 

TRMTGO, TGE1, TGE2AL, WFILT, ZFILT, LAM, 
LAMD, IBURN1, ACQD, PITER, YAWER 

ESTATE, TRACK, TERM, TGIL 


C 

UPDATES  : 

D. 

SISSOM 

- 

CR 

# 

032 

c 

B. 

HILL 

- 

CR 

# 

030 

c 

B. 

HILL 

- 

CR 

« 

038 

c 

T. 

THORNTON 

- 

CR 

* 

043 

c 

T. 

THORNTON 

- 

CR 

# 

048 

c 

D. 

SMITH 

- 

CR 

# 

059 

c 

D. 

SMITH 

- 

CR 

# 

064 

c 

R. 

RHYNE 

- 

CR 

* 

068 

c 

D. 

SISSOM 

- 

CR 

# 

069 

c 

D. 

SMITH 

- 

CR 

* 

070 

c 

D. 

SMITH 

- 

CR 

# 

074 

c 

R. 

RHYNE 

- 

CR 

t 

079 

c 

B. 

HILL  / 

- 

CR 

* 

081 

c 

R. 

RHYNE 

c 

B. 

HILL 

- 

CR 

# 

086 

c 

R. 

RHYNE 

- 

CR 

* 

087 

c 

R. 

RHYNE 

- 

CR 

* 

088 

c 

D. 

SISSOM 

- 

CR 

* 

091 

c 

B. 

HILL 

- 

CR 

* 

093 

IMPLICIT  DOUBLE  PRECISION 
IMPLICIT  DOUBLE  PRECISION 


REAL 

SNRO 

DOUBLE 

PRECISION 

CSSHFT (3) 

DOUBLE 

PRECISION 

LAMSEK ( 2 ) 

DOUBLE 

PRECISION 

LAM  (2) 

DOUBLE 

PRECISION 

RATE  (6) 

REAL 

VRELO ( 3) 

REAL 

RRELO (3) 

REAL 

TI2M (9) 

REAL 

MAGR 

REAL 

PITER 

REAL 

TGE2AL 

REAL 

TRMTGO 

DOUBLE 

PRECISION 

CMS (9) 

INTEGER 

SEKTYP 

INTEGER 

ESTATE 

C  LOCAL  DATA  USED  FOR  CONSTANTS, 

SAVE  IKALMN 

*  DATA  INITIALIZATION 
SINCLUDE ( ' A / INCLUDE /SSKALMAN . DAT ' ) 
SINCLUDE ( ' A / INCLUDE /SSCON1 1 . DAT ' ) 
SINCLUDE ( ' "/ INCLUDE/ SSCON1 2 . DAT' ) 
SINCLUDE ( ' " /INCLUDE /SSCON48 . DAT ' ) 
SINCLUDE ( ' " / INCLUDE /SSCON 50 . DAT' ) 
SINCLUDE ( "V INCLUDE /SSCON 5 5. DAT' ) 
SINCLUDE ( ' "/INCLUDE /SSCON 5 6 . DAT ' ) 
SINCLUDE ( ' ' ^ INCLUDE /SSCON 5 7 . DAT ' I 

DATA  IKALM.i  /  1.  / 


(A-H) 

( O— Z ) 

FRMRAT 

TMSHFT ( 3 ) 

,  TKSHFT  ( 3 ) 

LAMDXX (2) 

,  MAGRSQ 

LAMD (2) 

,  MAGRO 

LAMMO  (2) 

,  TI2MO ( 9) 

MAGV 

,  MAGRTR 

YAWER 

,  TGE1 

TGIL 

,  TGO 

,  ACQD 

,  TRACK  ,  TERM 

VARIABLES  AND  INITIALIZATION  FLAG 


IF  (IKALMN  . EQ  1)  THEN 
IKALMN  =  0 

IF  ( IFPAS  .EQ.  0)  THEN 

C  INITIALIZE  FILTER  PARAMETERS 

KFMODE  -  1 
TKF  =  T 


INITIALIZE  FILTER  ESTIMATES  Of  INERTIAL  FRAME  LAMBDA  AND 
LAMBDA  DOT 


PLMDH1 


(RRELO (3) *VRELO ( 1 ) 


RRELO(I)  *VRELO<3)  )  / 
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<RRELO(l)**2  *  RRELO(3)**2) 

PLAMDH  =  PLMDH1 

YLMDH1  =  (RRELO(l) *VRELO(2)  -  RRELO (2) *VRELO ( 1 ) ) / 
(RRELO(l)**2  +  RRELO  ( 2 )  *  *  2) 

YLAMDH  =  YLMDH1 

C  INITIALIZE  COVARIANCE  MATRIX  ELEMENTS 

PP22  =  SGP22**2 

PY22  =  SGP22**2 

PP1 2  =  SGP12**2 

PY12  =  SGP12**2 

PP11  =  SGP11**2 

PY11  =  SGP11**2 

C  INITIALIZE  PROCESS  NOISE  COVARIANCE 

RW  =  SGW**2 

C  INITIALIZE  MEASUREMENT  NOISE  MATRIX 

RV  =  AKSGME*ASIG**2 

ENDIF 
ENDIF 

C  INCREMENT  FILTER  PASS  COUNTER 

IFPAS  -  IFPAS  +  1 

C  PERFORM  EXECUTIVE  FUNCTION  FOR  SEEKER  TYPES  0  AND  1 

IF  (  SEKTYP.EQ.O  .OR.  3EKTYP.EQ.1  )  THEN 

C  INITIATE  ACQUISTION  MODE 

IF  (  ACQD.EQ.O  .AND.  MAGRTR . LE . RACQ  )  THEN 
ESTATE  =  0 

ACQD  =  1 

TRMTGO  -  TGO  -  (MAGR  -  RNGTRM) /MAGV 
TGIL  =  TRMTGO  +  TBWAIT 

TGE2AL  »  TGIL  +  DTVCS2 

CALL  OUTMES (0601, T, 0. 0D0) 

ENDIF 

C  COt.PUTE  THE  SEEKER  DATA  RATE 

IF  (  TRACK  .EQ.  1  )  THEN 
TRACK  -  2 

TGE1  -  TGO  -  (RNHITS  +  I LAG) /FRMRAT 
IBURN1  =  0 

CALL  OUTMES (0602, T, 0. 0D0) 

ELSEIF  (  TERM  .EQ.  1  )  THEN 
TERM  =  2 

CALL  OUTMES (0603, T, 0.OD0) 

ENDIF 

ENDIF 

C  USE  TRUE  LOS  ANGLES  AND  RATES  WITH  PERFECT  SEEKER  MODEL 

*  cannot  allow  this  partition  to  set  angular  errors,  even  i£  perfect 

*  seeker  is  used  sometime  in  future 

IF  (  SEKTYP.EQ.O  .AND.  ESTATE. EQ.0)  THEN 
LAMD(l)  =  LAMDXX(l) 

LAMD ( 2 )  ’  LAMDXX ( 2 ) 

*  PITER  =  LAMMO(l) 

*  YAWER  =  LAMMO ( 2 ) 

*  ROLLER  =0.0 
RETURN 

ENDIF 

C  DETERMINE  APPARENT  RELATIVE  INERTIAL  FRAME  STATES  FOR  LOCAL  USE 


RXI 

RRELO (1) 

RYI 

= 

RRELO (2) 

RZI 

= 

RRELO (3) 

VXI 


VP  L  1.0  <1 ) 


no  o  non 
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VYI  =  VRELCH2) 

VZI  =  VRELO ( 3 ) 

MAGRO  =  DSQRT  (  RXI"2  +  RYI"2  +  RZI**2  ) 

C  RECONSTRUCT  MEASURED  LOS  VECTOR  IN  SEEKER  FRAME 

TANPCH  »  DBLE  (  TAN  (  LAMMO(l)  )  ) 

TANYAW  =  DBLE  (  TAN  <  LAMMO ( 2 )  )  ) 

XLOSS  =  1.0D0  /  DSQRT  (  1.0D0  +  TANPCH"2  +  TANYAW*  * 2  ) 

YLOSS  =  XLOSS  *  TANYAW 

ZLOSS  =  -  XLOSS  *  TANPCH 

C  ROTATE  MEASURED  LOS  VECTOR  INTO  MISSILE  FRAME 


XLOSM  =  CMS (1) 'XLOSS  +  CMS (2) 'YLOSS  +  CMS (3) 'ZLOSS 
YLOSM  =  CMS (4) 'XLOSS  +  CMS (5) 'YLOSS  +  CMS (6) 'ZLOSS 
ZLOSM  =  CMS (7) 'XLOSS  +  CMS (8) 'YLOSS  +  CMS (9) 'ZLOSS 


C  ROTATE  MEASURED  LOS  VECTOR  INTO  INERTIAL  FRAME 


XLOSI  «  TI2MO(l) 'XLOSM  +  TI2MO (2) ' YLOSM  +  TI 2MO ( 3 ) * ZLOSM 
YLOSI  =  TI2MO (4) 'XLOSM  +  TI2MO ( 5 ) 'YLOSM  +  TI2MO ( 6) 'ZLOSM 
ZLOSI  =  TI2MO (7) 'XLOSM  +  TI2MO ( 8 ) * YLOSM  +  TI2MO ( 9) 'ZLOSM 

C  DETERMINE  MEASURED  LOS  ANGLES  IN  INERTIAL  FRAME 


PLAMM  «  DATAN2  (  -ZLOSI  ,  XLOSI  ) 

YLAMM  =  DATAN2  (  YLOSI  ,  XLOSI  ) 

C  EXECUTE  FILTER  INITIALIZATION  LOGIC  ON  FIRST  FILTER  PASS 


THE  FOLLOWING  INITIALIZATION  IS  DONE  HERE,  RATHER  THAN  IN  THE 
INITIAL  SECTION  TO  AVOID  REPETITIVE  CALCULATIONS  TO  OBTAIN  THE 
VALUES  OF  PLAMM  AND  YLAMM 

IF  (  IFPAS.EQ.l  )  THEN 

PLAMH1  =  PLAMM 
PLAMH  =  PLAMH1 
YLAMH1  =  YLAMM 
YLAMH  *  YLAMH1 

ENDIF 

DETERMINE  TIME  SINCE  LAST  FILTER  UPDATE 

IF  (  T.GT.TKF  )  THEN 
DTKF  =  T  -  TKF 
ELSE 

DTKF  =  0.0D0 
ENDIF 

TKF  -  T 

ENABLE  FIRST  BURN  WHEN  DATA  RATE  IS  SUFFICIENT  (SEEKER  TYPE  2) 

OR  WHEN  IN  TERMINAL  MODE  SEEKER  TYPE  3) 

IF  (  (SEKTYP . EQ . 2 . AND . FRMRAT .GE . RATE (5) . AND . IDRTOK . EQ . 0 )  .OR. 
(SEKTYP.EQ.3  .AND.  IDRTOK. EQ.O  .AND.  MTERM.LQ.l)  )  THEN 
TGE1  «  TGO  -  RNH ITS /FRMRAT 
IBURN1  -  0 
IDRTOK  «  1 
ENDIF 

C  ENABLE  ACQUISITION  MODE  ON  FIRST  PASS 

IF  (  (SEKTYP. NE. 3  .AND.  KFMODE . EQ . 1  .AND.  SNRO . GE . SNRACQ)  .OR. 
(SEKTYP.EQ.3  .AND.  KFMODE. EQ . 1  .AND.  MACQ.EQ.l)  )  THEN 
CALL  OUTMES (0604, T, MAGRO) 

KFMOOE  =  2 
ACQD  =  1 

ELSEIF  ( (SEKTYP. NE. 3  .AND.  KFMODE. EQ. 2  .AND.  SNRO. GE . SNRTRK )  .OR 
(SEKTYP.EQ.3  .AND.  KFMODE. EQ. 2  .AND.  MACQ.EQ.l)  )  THEN 

C  REINITIALIZE  ERROR  COVARIANCE  DIAGONAL  ELEMENTS  SWITCH  FROM 

C  ACQUISITION  TO  TRACK  MODE 

CALL  OUTMES (0605, T, MAGRO) 

KFMODE  =  3 


non  on  n  on  non  on 
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MAGRSQ  =  MAGRO* *2 
TGOSQ  =  TGO**2 

PP11  -  PP11  +  TKSHFT ( 3 ) *  *  2 /MAGRSQ 
PY11  =  PY1 1  +  TKSHFT (2) **2/MAGRSQ 
PP22  =  PP22  +  TKSHFT (3) **2/ (MAGRSQ-TGOSQ) 

PY22  =  PY22  +  TKSHFT (2) **2/ (MAGRSQ'TGOSQ) 

ENDIF 

IF  (  KFM0DE.GE.3  .AND.  IFPAS . GE . IDNINT (RNHITS)  )  ESTATE  =  0 

REINITIALIZE  ERROR  COVARIANCE  DIAGONAL  ELEMENTS  AT  SWITCH  FROM 
TRACK  TO  DISCRIMINATION  MODE 

IF  (  (SEKTYP.NE.3  .AND.  KFMODE.EQ.3  .AND.  SNRO.GE.SNRCSO)  -OR. 
(SEKTYP.EQ.3  .AND.  KFMODE.EQ.3  .AND.  MCSO.EQ.l)  )  THEN 
CALL  OUTMES (0606, T, MAGRO) 

KFMODE  =  4 
MAGRSQ  =  MAGRO* *2 
TGOSQ  =  TGO**2 

PP11  =  PP11  +  CSSHFT (3) *  *  2 /MAGRSQ 
PY11  =  PY11  +  CSSHFT (2) **2/MAGRSQ 
PP22  =  PP22  +  CSSHFT (3) **2/ (MAGRSQ*TGOSQ) 

PY22  =  PY22  +  CSSHFT (2) **2/ <MAGRSQ*TGOSQ) 

ENDIF 

REINITIALIZE  ERROR  COVARIANCE  DIAGONAL  ELEMENTS  AT  SWITCH  FROM 
DISCRIMINATION  TO  TERMINAL  MODE  (SEEKER  TYPE  2)  OR  FRAME  RATE 
EQUALS  12.5  (SEEKER  TYPE  3)  AND  ENABLE  SECOND  BURN 

IF  (  (SEKTYP.NE.3  .AND.  KFMODE. EQ. 4  .AND.  SNRO . GE . SNRTRM)  .OR. 

(SEKTYP.EQ.3  .AND.  KFMODE . EQ . 4  .AND.  FRMRAT. GE . RATE  (3 )  )  )  THEN 
CALL  OUTMES (0607, T, MAGRO) 

KFMODE  =  5 

TGE2AL  =  TGO  -  RNHITS /FRMRAT 
TRMTGO  =  TGO  -  RNHITS /FRMRAT 
MAGRSQ  =  MAGRO* *2 
TGOSQ  =  TGO* *2 

PP11  =  PP11  +  TMSHFT ( 3) *  *  2 /MAGRSQ 
PY11  =  PY11  +  TMSHFT (2) **2/MAGRSQ 
PP22  «  PP22  +  TMSHFT (3) **2/ (MAGRSQ*TGOSQ) 

PY22  =  PY22  +  TMSHFT (2) **2/ (MAGRSQ*TGOSQ) 

ENDIF 

COMPUTE  R  (  MEASUREMENT  NOISE  MATRIX  )  FOR  CURRENT  TIME 
RV  =  AKSGME  *  ASIG**2 

PROCESS  NOISE  TERMS  AS  A  FUNCTION  OF  HOMING  PHASE 

IF  (  KFMODE. GT. 2  .AND.  KFMODE. LT . 5  )  THEN 

RW  =  SGWH*  *2 

ELSE  IF  (  KFMODE. EQ. 5  )  THEN 
RW  =  SGWT**2 

ENDIF 

COMPUTE  Q  (  PROCESS  NOISE  MATRIX  )  FOR  CURRENT  TIME 

Qll  =  RW  *  DTKF*  *  2  /  4 . 0D0 

Q12  =  RW  *  DTKF  /  2.0D0 

Q22  =  RW 


EXTRAPOLATE  COVARIANCE  MATRIX  TO  CURRENT  TIME 
PIN+l)  =  PHI (N) *P (N) *PHI (N) T  +  Q 


PPX 

=  PP 1 2 

+  DTKF*PP22 

PYX 

=  PY12 

+  DTKF*PY22 

PP11 

=  Qll  + 

PP11  f  DTKF* (PP12+PPX) 

PY1 1 

=  Qll  * 

PY11  +  DTKF* (PY12+PYX) 

PP12 

-  Q12  + 

PPX 

PY12 

=  Q12  + 

PYX 

PP22 

=  Q22  + 

PP22 

PY22 

=  Q22  * 

PY22 

COMPUTE 

KALMAN 

FILTER  GAIN  MATRIX  : 

K  (N) 

=  P  (N> 

*HT*(  H*P(N)  * HT  +  RV  )**-l 

DNP 

=  PP11 

*•  RV 

DNY 

»  PY11 

+  RV 

AKP11 

«  °P11 

/  DNP 

AKY1I 

«  PY11 

/  DNY 
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AKP21  =  PP12  /  DNP 

AKY21  =  PY12  /  DNY 

IF  (  AKP11.GT.GFLIM  )  AKP11  =  GFLIM 

IF  (  AKY11 .GT. GFLIM  )  AKY11  =  GFLIM 

IF  (  AKP21 .GT.GFDLIM  )  AKP21  =  GFDLIM 

IF  (  AKY21 .GT.GFDLIM  )  AKY21  =  GFDLIM 

C  COMPOTE  FILTER  BANDWIDTH  AND  DAMPING 

IF  (  AKP21.GT.0.0D0  .AND.  DTKF . GT . 0 . 0D0  )  THEN 
WFILT  =  DSQRT  (  AKP21  /  DTKF  ) 

ZFILT  =  AKP11  *  WFILT  /  (  2.0D0  *  AKP21  > 

ENDIF 

C  UPDATE  COVARIANCE  MATRIX  : 

C  + 

C  P  (N)  =  <  I  -  K  IN)  *H  )  *  P  <N) 

PP22  -  PP22  -  AKP21*PP12 

PY22  =  PY22  -  AKY21*PY12 

PP12  =  PP12  -  AKP21*PP11 

PY12  -  PY12  -  AKY21*PY11 

PP11  =  PP11  -  AKP11*PP11 

PY11  «  PY11  -  AKY11*PY11 

C  ESTIMATE  DELTA  LOS  ANGULAR  RATE  DUE  TO  MISSILE  MOTION  (  ‘PLANT1 

C  INPUT  OR  FORCING  FUNCTION  ) 

PLMDF  »  (  RZI'VXI  -  RXI*VZI  )  /  (  RXI*'2  +  RZI*»2  ) 

YLMDF  =  (  RXI-VYI  -  RYI'VXI  )  /  (  RXI**2  +  RYI**2  ) 

IF  (  DTKF.NE.O.ODO  )  THEN 

DLPLMD  =  (  PLMDF  -  PLMDFP  ) 

DLYLMD  =  (  YLMDF  -  YLMDFP  ) 

ELSE 

DLPLMD  =  0.0D0 

DLYLMD  -  0.0D0 

ENDIF 

PLMDFP  =  PLMDF 

YLMDFP  »  YLMDF 

C  EXTRAPOLATE  FILTERED  INERTIAL  FRAME  STATES  TO  CURRENT  TIME 

PLAMH1  =  PLAMH  +  DTKF  *  (  PLAMDH  +  0 . 5 DO* DTKF “DLPLMD  ) 

YLAMH1  =  YLAMH  +  DTKF  *  (  YLAMDH  +  0 . 5D0*DTKF*DLYLMD  ) 

PLMDH1  =  PLAMDH  +  DLPLMD 

YLMDH1  =  YLAMDH  +  DLYLMD 

C  REVISE  FILTER  ESTIMATES  OF  INERTIAL  FRAME  LAMBDA  AND  LAMBDA  DOT  : 

C  “  +  * 

C  X  <  N )  =  X  ( N )  +  K(N)*(  YIN)  -  H*X(N)  ) 

ERRP  =  PLAMM  -  PLAMH1 

ERRY  =  YLAMM  -  YLAMH1 

PLAMH  =  PLAMH1  +  AKP11*ERRP 

PLAMDH  =  PLMDHI  +  AKP21*ERRP 

YLAMH  =  YLAMH 1  +  AKY1 1 *ERRY 

YLAMDH  -  MLMDH1  +  AKY21 *ERRY 

C  EXTRAPOLATE  LOS  ANGLES  AHEAD  TO  ACCOUNT  FOR  SIGNAL  PROCESSING  LAG 

IF  (  DTKF.NE.O.ODO  )  THEN 

DLPLMD  =  DLPLMD  *  SPLAG  /  DTKF 

DLYLMD  =  DLYLMD  *  SPLAG  /  DTKF 

ELSE 

DLPLMD  =  O.CDO 

DLYLMD  =  0.0D0 

ENDIF 

PLAMF  =  PLAMH  +  SPLAG  *  (  PLAMDH  *  0 . 5D0*SPLAG*DLPLMD  ) 

YLAMF  «  YLAMH  +  SPLAG  *  (  YLAMDH  +  0 . 5D0*SPLAG*DLYLMD  ) 

PLAMOF  -  PLAMDH  *  DLPLMD 

YLAMDF  -  YLAMDH  *  DLYLMD 

C  RECONSTRUCT  FILTERED  LOS  VECTOR  IN  INERTIAL  FRAME 

TANPCH  »  DTAN  (  PLAMF  ) 
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TANYAW  »  DTAN  (  YLAMF  ) 

COSPSQ  =  DCOS  (  PLAMF  )  ’*2 

COSYSQ  =  DCOS  (  YLAMF  )  **2 

XLOSI  =  1.0D0  /  DSQRT  (  1 . 0D0  +  TANPCH“2  +  TANYAW**2  ) 

YLOSX  =  XLOSI  *  TANYAW 

ZLOSI  =  -  XLOSI  *  TANPCH 

C  DETERMINE  FILTERED  LOS  VECTOR  RATES  IN  INERTIAL  FRAME 

XLOSDI  =  -  (  PLAMDF' TANPCH /COSPSQ 

+  YLAMDF ‘TANYAW /COSYSQ  )  *  XLOSI“3 
YLOSDI  -  YLAMDF’XLOSI  /COSYSQ  +■  XLOSDI  *TANYAW 

ZLOSDI  =  -  PLAMDF* XLOSI  /COSPSQ  -  XLOSDI ‘TANPCH 

C  ROTATE  LOS  VECTOR  INTO  MISSILE  FRAME 

XLOSM  =  TI2M (1 ) ‘XLOSI  -  TI2M ( 4 ) ' YLOSI  +  TI2M ( 7 ) * ZLOSI 
YLOSM  =  TI2M (2) ‘XLOSI  +  TI2M < 5 > 'YLOSI  +  TI2M ( 8 ) * ZLOSI 
ZLOSM  -  TI2M (3) ‘XLOSI  +  TI2M (6) ‘YLOSI  +  TI2M ( 9) ‘ZLOSI 

C  ROTATE  LOS  VECTOR  RATES  INTO  MISSILE  FRAME 

XLOSDM  =  TI2M(1) ‘XLOSDI  +  TI2M ( 4 ) ‘YLOSDI  +  TI2M (7 )• ZLOSDI 
YLOSDM  =  TI2M<2) ‘XLOSDI  +  TI2M ( 5 ) ‘YLOSDI  +  TI2M ( 8 )» ZLOSDI 
ZLOSDM  =  TI2M (3) 'XLOSDI  +  TI2M ( 6) 'YLOSDI  +  TI2M ( 9) 'ZLOSDI 

C  ROTATE  LOS  VECTOR  INTO  SEEKER  FRAME 

XLOSS  -  CMS (1) ‘XLOSM  +  CMS (4) 'YLOSM  +  CMS (7) 'ZLOSM 
YLOSS  «  CMS (2) 'XLOSM  +  CMS (5) ‘YLOSM  +  CMS (8) ‘ZLOSM 
ZLOSS  =  CMS (3) ‘XLOSM  +  CMS (6) 'YLOSM  +  CMS (9) ‘ZLOSM 

C  ROTATE  LOS  VECTOR  RATES  INTO  SEEKER  FRAME 

XLOSDS  -  CMS (1) ‘XLOSDM  +  CMS ( 4 ) 'YLOSDM  +  CMS (7) 'ZLOSDM 
YLOSDS  -  CMS (2) ‘XLOSDM  +  CMS ( 5 ) 'YLOSDM  +  CMS ( 8 )' ZLOSDM 
ZLOSDS  -  CMS (3) 'XLOSDM  +  CMS ( 6) 'YLOSDM  +  CMS ( 9) 'ZLOSDM 

C  DETERMINE  LOS  ANGLES  IN  SEEKER  FRAME 

LAM (1)  -  DATAN2  (  -ZLOSS  ,  XLOSS  ) 

LAM (2)  =  DATAN2  (  YLOSS  ,  XLOSS  ) 

C  DETERMINE  LOS  ANGULAR  RATES  IN  SEEKER  FRAME 

TANPCH  -  DTAN  (  LAM ( 1 )  ) 

TANYAW  =  DTAN  (  LAM (2)  ) 

COSPSQ  =  DCOS  (  LAM  ( 1 )  )  "2 

COSYSQ  -  DCOS  (  uAM  ( 2 )  )  “2 

LAMD(l)  =  (  -  ZLOSDS  -  XLOSDS'TANPCH  )  *  COSPSQ  /  XLOSS 

LAMD ( 2 )  -  (  YLOSDS  -  XLOSDS 'TAN YAW  )  ‘  COSYSQ  /  XLOSS 

C  DETERMINE  ATTITUDE  ERRORS 

IF  (  ESTATE  .EQ.  0  )  THEN 
PITER  =  LAM ( 1 ) 

YAWER  =  -LAM  (2) 

*  following  line  moved  to  partition  with  MCGUID 
'  ROLLER  =0.0 

ENDIF 

RETURN 

END 


FILE:  uuv22.19g/duti 1 it y /uumasspr .for 


C - 

SUBROUTINE  MASSPR (T, MDOTT, MDOTF, MDOTA, MDOTV, MASS, EISP, T3RK , IMASS, 
MDOT, WEIGHT, WDOTTP, WDCTFR, WDOTKV, WDOTTI , CG, I XX, 
IYY, IZZ) 

c - 

c 

C  SUBROUTINE  NAME 

C 

C  AUTHOR  (S)  : 

C 

C  FUNCTION  : 


MASSPR 
B.  HILL 

CALCULATE  MISSILE  MASS  PROPERTIES 
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CALLED  FROM  : 
SUBROUTINES  CALLED 
INPUTS  : 

OUTPUTS  : 


MAIN 


TABLE 


T, MDOTT, MDOTF, MDOTA, MDOTV, MASS, EISP 

MDOT, WEIGHT, WDOTTP,  WDOTFR, WDOTKV, WDOTTI, CG, 


c 

IXX, IYY, IZZ 

c 

c 

BOTH  : 

TBRK, IMASS 

c 

UPDATES  : 

D. 

SMITH 

-  CR 

* 

059 

c 

D. 

SISSOM 

-  CR 

« 

069 

c 

D. 

SMITH 

-  CR 

# 

076 

c 

D. 

SMITH 

-  CR 

# 

080 

c 

B. 

HILL  / 

-  CR 

* 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

-  CR 

# 

087 

c 

B. 

HILL 

-  CR 

* 

089 

c 

B. 

HILL 

-  CR 

# 

093 

IMPLICIT  DOUBLE 

PRECISION 

(A-H) 

IMPLICIT  DOUBLE 

PRECISION 

(O-Z) 

REAL 

CG<3) 

,  EISP 

REAL 

CGX (20) 

,  CGY ( 20 ) 

REAL 

CGZ (20) 

,  INERXX (20) 

,  INERYY (20) 

REAL 

INERZZ (20) 

REAL 

IXX 

,  IYY 

,  IZZ 

DOUBLE  PRECISION 

MASS 

REAL 

SNGLMASS 

DOUBLE  PRECISION 

MDOT 

REAL 

MASST1 (20) 

,  MASST2 (20) 

REAL 

MDOTA 

,  MDOTF 

,  MDOTT 

REAL 

MDOTV 

C  LOCAL  DATA  USED  TO  HOLD  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IDATIN  ,  BISP 


*  DATA  INITIALIZATION 
S INCLUDE ( ' VINCLUDE/SSMASSPR.DAT' ) 

S INCLUDE ( 1  * / INCLUDE /SSCON2 2 . DAT  1 ) 

S INCLUDE ( ’ A/INCLUDE/SSCCN23 . DAT ' ) 

SINCLUDE { ' "V  INCLUDE/ SSCON4 5 . DAT ' ) 

S INCLUDE ( ' A/INCLUDE/SSCON58.DAT' ) 

DATA  IDATIN  /  1  / 

DATA  ICG  /  1  /,  III  /  1  / 

IF  ( I MASS  . EQ.  1)  THEN 

IMASS  =>  0 

IF  (IDATIN  .EQ.  I)  THEN 
IDATIN  -  0 

IF  (T  .LE.  TSTG1 )  THEN 
BISP  =  BISP1 

EISP  =■  BISP'WPROPl/ (WPROP1  *  WIN'Sl) 

ELSEIF  (T  .LE.  TSTG2 )  THEN 
BISP  -  BISP2 

EISP  =  BISP*WPROP2/  (WPRGP2  )•  WINS2 ) 

ELSE 

BISP  =  0.0 
EISP  =  0.0 
ENDIF 

ELSEIF  (T  .LE.  TSTG2 )  THEN 

C  CALCULATE  BOOSTER  SPECIFIC  IMPULSE  AT  FIRST  STAGE 

C  SEPARATION 


BISP  =  BISP2 

EISP  =  BISP-WPROP2/ (WPRCP2  *  WIN32) 
ELSE 
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C  ZERO  BOOSTER  SPECIFIC  IMPULSE  AFTER  SECOND  STAGE 

BISP  =  0.0 
EISP  =  0.0 
ENDIF 

ENDiF 

C  CALCULATE  TOTAL  MASS  FLOW  RATE 

MDOT  =  -  MDOTT  -  MDOTF  -  MDOTA  -  MDOTV 
C  CONVERT  MASS  TO  WEIGHT 

WEIGHT  =  MASS*XMTOF 

C  CALCULATE  WEIGHT  EXPULSION  RATES 

IF  (  T.LE.TSTG2  )  THEN 

WDOTTP  =  -MDOTT *XMTOE* EISP /BISP 
WDOTTI  =  MDOTT *XMTOF* EISP 
ELSE 

WDOTTP  =0.0 
WDOTTI  =  0.0 
ENDIF 

WDOTFR  =  MDOTF'XMTOF 

WDOTKV  =  (-MDOTF  -  MDOTA  -  MDOTV) *XMTCF 
C  CALCULATE  MISSILE  CENTER  OF  GRAVITY  COMPONENTS 

SNGLMASS  =  SNGL (MASS) 

CALL  TABLE (MASSTl.CGX, SNGLMASS, CG ( 1 ) ,20, ICG) 

CALL  TABLE (MASSTI,CGY, SNGLMASS, CG<2)  ,  20,  ICG) 

CALL  TABLE (MASST1, CGZ, SNGLMASS, CG (3) , 20, ICG) 

C  CALCULATE  MISSILE  MOMENT  OF  INERTIA 

CALL  TABLE (MASST2, INERXX, SNGLMASS, IXX,  20,111) 

CALL  TABLE (MASST2, INERYY, SNGLMASS, IYY, 20, III) 

CALL  TABLE (MASST2 , INERZZ, SNGLMASS, IZZ, 20, III) 

RETURN 

END 


FILE:  uuv22 . 1 9g/dut i 1 i ty /uumcauto . for 


C 


C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 


SUBROUTINE  MCAUTO (T, IXX, IYY, IZZ, SP, SQ, SR, ROLLER, PITER, YAWER, IDIST, 
IACSON, IBURND, IBURNM, IDMEAS, IPASSM, ICMD, TRATON, 
TPATON, TYATON, DTSAMP, TSAL, TSAH, TLAPS , ITHRES, 

ANVP , ACSLEV, TMAUTO) 


SUBROUTINE 

NAME  : 

MCAUTO 

AUTHOR  : 

R.  RHYNE 

FUNCTION  : 

GENERATES  ACS  COMMANDS  TO  NULL  LARGE 
ATTITUDE  ERRORS  AND  RATES  DURING  MIDCOURSE 

CALLED  FROM 

FORTRAN  MAIN 

SUBROUTINES 

CALLED  : 

NONE 

INPUTS  : 

T, IXX, IYY, IZZ, SP, SQ, SR, ROLLER, PITER, 

YAWER, IDIST, IACSON, IBURND, IBURNM, IDMEAS 

C  OUTPUTS 

C 

C 

C  BOTH  : 


ICMD, TRATON, TPATON, TYATON , DTSAMP , TSAL, 
TLAPS, ITHRES, ANVP, ACSLEV,  TMAUTO 

IPASSM 


C 


c 

UPDATES  : 

B. 

HILL  / 

-  CR 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH 

-  CR 

# 

082 

c 

R. 

RHYNE 

-  CR 

* 

083 

c 

R. 

RHYNE 

-  CR 

C87 

c 

R. 

RHYNE 

♦  CR 

« 

090 

TSAH, 


oooo 
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D.  SMITH  -  CR  f  091 
B.  HILL  -  CR  i  C93 


IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (0-2) 


DOUBLE  PRECISION 

DOUBLE  PRECISION 

DOUBLE  PRECISION 

REAL 

REAL 

REAL 

REAL 


II  (3) 
OMEGA (3) 
AERROR (3) 
IXX 
SP 

ROLLER 
ACS LEV 


ANoACL ( 3 , 4,1 
TBURN A (3) 
OMEGAD 
IVY 
SQ 

PITER 


INTEGER  IMCPAS (3,4) 

*  DATA  INITIALIZATION 
S INCLUDE  ( •'VINCLUDE/SSMCAUTO.DAT'  ) 

S INCLUDE ( ' " /INCLUDE/SSCON5  9 . DAT ' ) 
SINCLUDE ( ' " /INCLUDE /SSCON60 . DAT  1 ) 

S INCLUDE ( ' A/INCLUDE/SSCON01 . DAT ' ) 
SINCLUDE (  "'/ INCLUDE /SSCON02. DAT' ) 
SINCLUDE ( 1 ^ /INCLUDE /SSCONO 5 . DAT ' ) 
SINCLUDE ( *  'V INCLUDE /SSCONO 7 , DAT ' ) 
SINCLUDE  (  1  ''/INCLUDE /SSCONO 8  .  DAT  1  ) 
SINCLUDE ( • * / INCLUDE /SSC0N1 9 . DAT ' ) 


IF  (  IPASSM.EQ.O  )  THEN 

C  INITIALIZE  ACCELERATION  TABLE,  PULSE  FLAGS, 


3 

4 


10 


MOMARM(l) 

=  RIARM 

MOMARM (2) 

-  PI ARM 

MOMARM (3) 

=  Y I ARM 

II  (1) 

=  IXX 

II  (2) 

=  IYY 

11(3) 

=  IZZ 

DO  10  I  = 

1,  3 

2.*ACSFL*MCMARM(I) /II (I) 
2.*ACSFH*MOMARM  (I) /II  (I) 


ANGACL  (I,  1,1) 

ANGACL (1,2,1) 

IF  (  I.EQ.l  )  THEN 
ANGACL (I, 3, 1)  = 

ANGACL (I, 4, 1)  = 

ELSE 

ANGACL (I, 3, 1)  =  0 . 

ANGACL (I, 4, .)  =  0. 

ENDIF 

DO  4  J  =  1, 4 

IMCPAS (I, J)  =  1 

AACCEL ( I , J)  -  ANGACL (I, J, 1) 

DO  3  K  =  2,10 

ANGACL  (I, J, K)  =  0. 
CONTINUE 


ACSFL*MOMARM ( I ) /II 
ACSFH*MOMARM(I) /II 


CONTINUE 
CONTINUE 
IPASSM  -  l 
ICNT  =  0 
IP2END  =  1 
ICOAST  =  1 
TP2END  -  1000.0 
TP3END  =  1000.0 
TCOAST  *  1000.0 
TRDONE  =  1000.0 


ENDIF 


TIME  SINCE  LAST  CALL 

DTMCA  =  T  -  TLSTMA 
TLSTMA  =  T 


C 


DETERMINE  IF  CORRECTION  REQUIRED  AND  ISSUE  APP> 

IF  (  ICMD.EQ.  0  .AND.  IDISI.r'J.O 

•AND.  IBURNM.SE. 0  .AND.  I BDRND . EQ . I  ) 

IF  (  A33  (ROLLER)  .  GE.CArriL  )  THEN 


:rrect:on  bur:. 


),  OMEGA I  (3 ) 

,  MOMARM  ( 3 ) 

,  AACCEL (3, 4) 
,  IZZ 
,  SR 
,  YAW ER 


AND  PULSE  TIMES 


(I) 

(I) 


OPR  I ATE  COMMAND 


COMPUTE  INITIAL  R; 
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ICMD  =  1 
IVPFL  =  3 
IACSB1  =  1 

IF  (  ABS (ROLLER) . GE . 4 . 'CAPHL  )  IVPFL  =  2 
OMEGAD  =  ROLLER*AACCEL(l, IVPFL) /ABS (ROLLER) 

IF  (  SP7R0LLER.LT. 0.  )  THEN 

RLLERO  =  ROLLER  +  S? ' *  2/ ( 2 . -OMEGAD) 

ELSE 

RLLERO  =  ROLLER 
ENDIF 

TBACS  =  DSQRT  (DABS  (RLLERO  /  (2.  'AACCEL  C.,  IVPFL)  )  ) 
ELSEIF  (  ABS (SP) .GT.CRPHL  )  THEN 

DEFINE  ROLL  RATE  CORRECTION  COMMAND 


ICMD  =  1 
IRATE  =  1 
IACSB1  =  1 

IF  (  ABS(SP) .GT.TSO.'CRPH  )  THEN 
IVPFL  =  4 

ELSEIF  (  ABS(SP) . GT . 37 5 . ’CRPH  )  THEN 
IVPFL  =  2 

ELSEIF  (  ABS (SP) .GT. 15. *CRPH  )  THEN 
IVPFL  =  3 
ELSE 

IVPFL  =  1 
ENDIF 

ELSEIF  (  IDMEAS.NE.2  )  THEN 

IF  (  ABS (PITER) .GT.CATHL  !  THEN 

COMPUTE  INITIAL  PITCH  CORRECTION  BURN  TIME 

OMEGAD  =  PITER*AACCEL (2, 2) /ABS (PITER) 

IF  (  SQ/P ITER . LT . 0 .  )  THEN 

PITERO  =  PITER  +  SQ"2/ (2. 'OMEGAD) 

ELSE 

PITERO  =  PITER 
ENDIF 

TBACS  =  DSQRT ( DABS (PITERO) / (2. * AACCEL (2,2))) 

ISSUE  PITCH  COMMAND 

ICMD  =  2 
IVPFL  =  2 
IACSB1  =  1 

ELSEIF  (  ABS ( lAWER) .GT.CAPSL  )  THEN 

OMEGAD  =  YAWER'AACCEL (3,2) /ABS ( YAWER) 

IF  (  SR/ YAWER . LT . 0 .  )  THEN 

YAWERO  =  YAWER  '  SR"  2/ 1 2 . 'OMEGAD) 

ELSE 

YAWERO  =  YAWER 
ENDIF 

TBACS  =  DSQRT (DABS (YAWERO) / (2. 'AACCEL (3, 2) > ) 

ISSUE  YAW  COMMAND 

ICMD  =  3 
IVPFL  =  2 
IACSB1  =  1 

ELSEIF  (  TSAH . GT . T'TSMPH+EPSL  .AND.  IDMEAS.EQ.I 

ENABLE  KV  AUTOPILOT 

TSAL  T 

TSAH  =  T 
TLAPS  =  T 
ENDIF 

ELSEIF  (  IBURND.EQ.C  )  THEN 

NULL  BODY  RATES  BEFORE  DISTURBANCE  PULSE  ISSUED 

IF  (  ABS (SQ) .GE.CRTH  )  THEN 
ICMD  =  2 
IVPFL  =  1 


-  SP/OMEGAD 


-  SQ/OMEGAD 


-  SR/OMEGAD 


)  THEN 
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IF  (  ABS(SQ)  .GT.35.'CRTh'  )  IVPFL  =  2 
IRATE  =  I 
IACSB1  =  1 

ELSEIF  {  ABS (SR) .GE.CRPS  )  THEN 
ICMD  «  3 
IVPFL  =  1 

IF  (  ABS(SR) ,GT.35.*CRPS  >  IVPFL  =  2 
IRATE  -  1 
IACSB1  =  1 
ENDIF 

ENDIF 

ENDIF 


C  EXECUTE  CONTROL  LOGIC  IF  ATTITUDE/RATE  CORRECTION  REQUIRED 

IF  (  ICMD.NE.O  )  THEN 

C  ZERO  ACS  BURN  VECTC-  AND  FORM  ANGULAR  RATE  AND  ERROR  VECTORS 

TBURNA ( 1 )  =  0. 

TBURNA (2)  =  0. 

TBURNA ( 3 )  =  0. 

OMEGA (1)  =  SP 
OMEGA (2)  =  SQ 
OMEGA (3)  =  SR 

AERROR ( 1 )  =  ROLLER 
AERROR (2)  =  PITER 
AERROR (3)  =  YAWER 

C  UPDATE  ANGULAR  ACCELERATION  TABLE 


12 


IF  (  IACSON.EQ.l  )  THEN 
ICNT  =  ICNT  +  1 

IF  (  ICNT.EQ.l  )  OMEGAI (ICMD)  =  OMEGA ( ICMD) 

IF  (  ICNT.GE.2  )  THEN 

DO  12  I  -  IMCPAS (ICMD, IVPFL) , 1, -1 

IF  (I.LT.10)  ANGACLUCMD,  IVPFL,  I  +  l)  = 

ANGACL (ICMD, IVPFL, I) 

CONTINUE 

ANGACL (ICMD, IVPFL, 1 ) =DABS (OMEGAI (ICMD) -OMEGA (ICMD) ) /DTMCA 
OMEGAI (ICMD)  “  OMEGA (ICMD) 

IMCPAS (ICMD, IVPFL)  =  IMCPAS ( ICMD, IVPFL)  +  1 
IF  (IMCPAS (ICMD, IVPFL)  GE.ISAMP)  IMCPAS ( ICMD, IVPFL) =1  SAMP 
ENDIF 
ELSE 

ICNT  =  0 
ENDIF 


C  COMPUTE  EXPECTED  ANGULAR  ACCELERATION 

AACCEL (ICMD, IVPFL)  =  0.0 
DO  20  I  =  1 , IMCPAS (ICMD, IVPFL) 

AACCEL (ICMD, IVPFL)  =  AACCEL (ICMD, IVPFL) +ANGACL (ICMD, IVPFL, I ) 
20  CONTINUE 

AACCEL (ICMD, IVPFL)  =  AACCEL (ICMD, IVPFL) / 

DBLE (IMCPAS (ICMD, IVPFL) ! 


C  EXECUTE  BURN  LOGIC 

IF  (  IRATE. EQ.l  )  THEN 
C  RATE  CORRECTION 


IF  (  IACSB1.EQ.1  )  THEN 

TBURNA (ICMD)  =  -OMEGA ( ICMD) /AACCEL ( ICMD, IVPFL) 
DTSAMP  =  DABS (TBURNA (ICMD) ) 

TRDONE  =  T  +  DTSAMP  +  TLAGA  +  TRCNA 

ITHRES  =  1 

IACSB1  =  0 

ICNT  =  0 

TSAL  =  1000. 

TSAH  =  1000. 

TLAPS  «  1000. 

ELSEIF  (  T.GE. TRDONE  )  THEN 
TRDONE  =  1000. 

IRATE  =  0 
ICMD  =  0 
ENDIF 
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ELSEIF  (  IACSB1.EQ.1  )  THEN- 

ENABLE  FIRST  ATTITUDE  CONTROL  PULSE 

TBURNA (ICMD)  =  AERROR (ICMD) 'T3ACS/DABS (AERROR (ICMD) ) 


DTSAMP 

DABS (TBURNA (ICMD) ) 

ITHRES 

- 

1 

TCOAST 

= 

T  +  DTSAMP  +  TLAGA  + 

ICOAST 

= 

0 

IACSB1 

= 

0 

ICNT 

= 

0 

TSAL 

= 

1000. 

TSAH 

= 

1000. 

TLAPS 

a 

1000. 

ELSEIF  (  T.GE.TCOAST  .AND.  ICOAST.EQ.O  )  THEN 

COMPUTE  SECOND  BURN  TO  LEAVE  DESIRED  LOW  LEVEL  BURN 

ICOAST  »  1 
IACS32  =  1 

IF  (  OMEGA  ( ICMD)  .T.T.O.  )  THEN 
DIRECT  =  -1. 

ELSE 

DIRECT  =  1. 

ENDIF 

IF  (  ICMD.EQ.l  .AND.  IVPFL. EC-2  )  THEN 
IVPFLN  =  3 
ELSE 

IVPFLN  -  1 
END.  F 

TBURN2= (OMEGA (ICMD ) -DIRECT 'AACCEL ( ICMD, IVPFLN) 'TBURN3) 

/AACCEL (ICMD, IVPFL) 

ELSEIF  ,  I . uE . TCOAST  .AND.  IACSB2.EQ.I  )  THEN 

ENABLE  ACS  BURN  WHEN  ATTITUDE  ERROR  EQUALS  EXPECTED 
DISTANCE  FROM  DESIRED  LOW  LEVEL  THIRD  PULSE  ERROR 

THET2D  =  OMEGA (ICMD)  -  AACCEL (ICMD, IVPFL) *TBURN2 
THT2DD  =  -DIRECT'AACCEL (ICMD, IVPFLN) 

THT1DD  =  -DIRECT'AACCEL (ICMD, IVPFL) 

DELANG  =  0.5*  (THET2D"2  -  OMEGA  (ICMD)  "2) /THT1DD  + 

2 . *THET2D'TBURN3  -  0.5'THET2D"2/THT2DD 
DELNXT  =  AERROR (ICMD)  -  OMEGA ( ICMD ) 'DTMCU 
IF  (  DABS (DELANG, . GE . DABS (DELNXT)  )  THEN 
IACSB2  =  0 

ICNT  «  0 

TBURNA (ICMD)  =  -TBURN2 
DTSAMP  =  DABS (TBURNA (ICMD) ) 

ITHRES  =  1 

IP2END  =  1 

TP2END  =  T  +  DTSAMP  +  TLAGA  +  TRDNA 

DELANG  =  0. 

ENDIF 

ELSEIF  (  T.GE.TP2END  .AND.  IP2END.EQ.1  >  THEN 

DEFINE  LOW  LEVEL  ACS  PULSE  FOR  'FINE  TUNING’ 

DELANG  =  0. 5*OMEGA(ICMD)  "2/AACCEL  (ICMD,  IVPFLN) 

DELNXT  -  AERROR (ICMD)  -  OMEGA ( ICMD) * DTMCU 

TDELAN  =  (DABS (AERROR (ICMD) )  -  DELANG) /DABS (OMEGA ( ICMD) ) 

IF  (  DELANG. GE. DABS (DELNXT)  .OR.  TDELAN . GT . 2 . 5'TBURN3  .OR. 
OMEGA ( ICMD ) /AERROR (ICMD) . LT . 0 .  )  THEN 
IP2END  =  0 

ICNT  =  0 

TBURNA (ICMD)  »  -OMEGA ( ICMD) /AACCEL ( ICMD, IVPFLN) 

DTSAMP  =  DABS (TBURNA (ICMD) ) 

ITHRES  =  1 

IVPFL  »  IVPFLN 

TP3END  =  T  +  DTSAMP  +  TLAGA  *  TRDNA 

ENDIF 

ELSEIF  (  T.GE.TP3END  !  THEN 

CORRECTION  COMPLETE  FOR  Ith  AXIS 

TP3END  «  1000. 

DELANG  -  0. 


n  n 
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ICMD  =  0 

ENDIF 
ENDIF 

DEFINE  ACS  LEVEL  AND  VALVE  PAIR  CONFIGURATION  BASED  ON 
ACCELERATION  TABLE  INDEX  USED 

IF  (  IVPFL.EQ.4  )  THEN 
ACS LEV  =  2. 

ANVP  =  2  . 

ELSEIF  (  IVPFL.EQ.3  )  THEN 
ACSLEV  =  1. 

ANVP  =  2 . 

ELSEIF  (  IVPFL.EQ.2  )  THEN 
ACSLEV  =  2. 

ANVP  =  1 . 

ELSE 

ACSLEV  -  i. 

AN  v  t  i - 

ENDIF 

C  UPDATE  ACS  BURN  COMMANDS 

TRATON  =  TBURNA(l) 

TPATON  =  TBURNA ( 2 ) 

TYATON  =  TBURNA (3) 

C  CALCULATE  NEXT  TIME  TO  CALL 

TMAUTO  =  T  +  DTMCU  -  EPSL 

RETURN 

END 


FILE :  uuv22 . 1 9g/dut i 1 i ty /uumi ssl t .for 


C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  MISSLT (T, QUAT, CIM, MASS, FXA, FXT, 

FRCX, FXACS, FXVCS, FYA, FYT, FRCY, FYACS, FYVCS, F2A, 
FZT, FRCZ, FZACS, FZVCS, 

X,  Y,Z,XD,  YD,ZD,UD,VD,WD, 

GB,  GR, MGR, FX,  FY, FZ, XDD, YDD, ZDD, MXYZDD, 
t ,V, W, PHI, THT, PSI  > 


SUBROUTINE  NAME  :  MISSILT 

AUTHOR (S)  :  D.  C.  FOREMAN,  A.  P.  BUKLEY 

FUNCTION  :  COMPUTES  THE  TRANSLATIONAL 

MISSILE  ACCELERATIONS 

CALLED  FROM  :  FORTRAN  MAIN 

SUBROUTINES  CALLED  :  FV2BXI 

INPUTS  :  T, QUAT, CIM, MASS, FXA, 

FXT, FRCX, FXACS, FXVCS, FYA, FYT, FRCY, FYACS, 
FYVCS, FZA, FZT, FRCZ, FZACS, FZVCS, 

X,  Y,  Z, XD, YD, ZD 

OUTPUTS  :  UD,  VD,  WQ,  PD,  QD,  RD,  GB,  GR,  MGR,  FX,  FY, 

FZ, XDD, YDD, ZDD, MXYZDD, U, V, W, QUATD, PHI, THT, 
PSI 


c 

UPDATES  : 

D. 

SISSOM 

- 

CR 

i 

Oil 

c 

T. 

THORNTON 

- 

CR 

i 

012 

c 

T. 

THORNTON 

- 

CR 

* 

018 

c 

B. 

HILL 

- 

CR 

# 

030 

c 

T. 

THORNTON 

- 

CR 

* 

031 

c 

T. 

THORNTON 

- 

CR 

* 

033 

c 

T. 

THORNTON 

- 

CR 

# 

035 

c 

T. 

THORNTON 

- 

CR 

# 

037 

c 

T . 

THORNTON 

- 

CR 

# 

049 

c 

T. 

THORNTON 

- 

CR 

* 

050 

c 

D. 

SMITH 

- 

CR 

059 

c 

D. 

SMITH 

- 

CR 

* 

060 

c 

B. 

HILL 

- 

CR 

it 

062 
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c 

D . 

SMITH 

-  CR 

n 

076 

c 

R. 

RHYNE 

-  CR 

# 

079 

c 

B. 

HILL  / 

-  CR 

if 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

-  CR 

* 

087 

c 

B. 

HILL 

-  CR 

* 

093 

c 

c 


IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 


REAL 

REAL 

REAL 


FRCX,  FRCY,  FRCZ,  FXA,  FYA,  FZA 
FXACS,  FYACS,  FZACS,  FXT,  FYT,  FZT 
FXVCS,  FYVCS,  FZVCS 


REAL 

DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
DOUBLE  PRECISION 
REAL 

DOUBLE  PRECISION 
DOUBLE  PRECISION 


CIM (9) 

GB  (3) 

GR  (3) 

I XX 
IZZ 
MXYZ 
MXYZDD 
PQRI3) 
QUAT ( 4 ) 
QUATD ( 4 ) 
UXYZDD (3) 


CMI ( 9)  ,  TMPI 

IYY 

MASS  ,  MGR 


UXYZ (3) 
XYZLCH (3) 


C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IMISL 

*  DATA  INITIALIZATION 

SINCLUDE  (  ■''/INCLUDE/SSMISSIL.DAT1  ) 

S INCLUDE) • VINCLUDE/SSCON28.DAT' ) 

SINCLUDE ( • VINCLUDE/SSCON39.DAT' ) 

$  INCLUDE ( ' * / INCLUDE/SSCON63 . DAT  1 ) 


DATA  IMISL  /  1  / 

DATA  NCLEAR  /  0  / 

IF  (IMISL  -EQ.  1)  THEN 

IMISL  =  0 

C  COMPUTE  MISSILE  LAUNCH  POSITION  IN  INERTIAL  FRAME 

CMI (1)  =  CIM (1) 

CMI  (2)  =  CIM  (4) 

CMI  (3)  =  CIM (7) 

CMI  (4)  =  CIM  (2) 

CMI (5)  =  CIM ( 5 ) 

CMI (6)  =  CIM (8) 

CMI (7)  =  CIM ( 3 ) 

CMI  (8)  =  CIM  (  6) 

CMI (9)  =  CIM (9) 

IF  (T  .EQ.  0.0)  THEN 

XYZLCH (1)  =  XLNCH*CMI(1)  +  RADE 
XYZLCH (2)  =  XLNCH*CMI ( 2 ) 

XYZLCH (3)  =  XLNCH*CMI (3) 

ENDIF 


ENDIF 


C  DETERMINE  LOCAL  GRAVITY  VECTOR 

MXYZ  =  DSQRT  (  X**2  ♦  Y**2  +  Z**2  ) 
MGR  -  GMU  /  MXYZ* *2 


IF  (  MXYZ.GT.0.0D0  )  THEN 
*  FTN286  X4 1 5  OPTIMIZED) 

99999  CONTINUE 

UXYZ (1)  -  X  /  MXYZ 
UXYZ  (2)  =  Y  /  MXYZ 
UXYZ  (3)  =  Z  /  MXYZ 
ELSE 

UXYZ  ( I )  =  0.0D0 
UXYZ  ( 2 )  =  0.0D0 
UXYZ  (3)  =  0 . 0D0 
ENDIF 
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C  CALCULATE  GRAVITY  VECTOR  IN  INERTIAL  AND  BODY  FRAMES 

gr;i;  -  -  mgr*'jxy2c; 

GR ( 2 )  =  -  MGR*UXYZ<2) 

GR ( 3 )  =  -  MGR*UXYZ(3) 

Geni)  »  CIM(1)*GR(1)  T  CIM(4)*GR(2)  +  CIM(7)«GR<3) 

GB  ( 2 )  -  CIM(2)*GR(1)  +  CIM(5)*GR(2)  +  CIM(8)*GR(3) 

GB  (3)  =  CIM  ( 3  )  *GR  ( 1 )  +  CIM(6)*GR(2i  +  CIM(9)*GR(3) 

C  CALCULATE  TOTAL  FORCES 


FX 

=  FXT 

+ 

FXA 

+ 

FRCX 

+  FXACS 

+ 

FXVCS 

FY 

»  FY” 

FYA 

+ 

r  RCY 

+  FYACS 

+ 

FYVCS 

FZ 

=  FZT 

FZA 

+ 

FRCZ 

+  FZACS 

+ 

FZVCS 

C  MISSILE  CLEARED  THE  LAUNCHER 


IF  (  NCLEAR.EQ. 1  )  THEN 

UD  =  FX/MASS  +  GB ( I ) 

VD  **  FY/MASS  +  GB ( 2 ) 

WD  =  FZ/MASS  +  GB ( 3 ) 

C  MISSILE  STILL  ON  GROUND 


ELSE  IF  (  FX/MASS. LE . DABS(GBtl) )  )  THEN 


GB  ( 1 ) 

= 

Q.O 

GB  ( 2 ) 

= 

0.0 

GB<3) 

= 

0.0 

GRID 

* 

o.c 

GR  ( 2 ) 

= 

o.c 

GR<3) 

= 

0 . 0 

UD 

= 

0.0 

VD 

= 

0.0 

WD 

= 

0.0 

MISSILE 

OFF 

GROUND  BUT  NOT  CLEAR  OF  THE  LAUNCHER 

ELSE  IF 

(  x. 

.  I.E .  XYZLCH  ( 1 )  .AND.  Y  .  LE  .  XYZLCH  ( 2 )  .AND 

z, 

.LE.XYZLCH (3)  )  THEN 

GB(2) 

= 

0.0 

GB  ( 3 ) 

= 

0.0 

GR  ( 1 ) 

= 

CMI  (1)  *GB(1) 

GR<2) 

= 

CMI (2) *  GB  1 1 ) 

GR<3) 

= 

CMi  (3)  'GB(l) 

UD 

= 

FX/MASS  +  GB ( 1 ) 

VD 

= 

0.0 

WD 

= 

0.0 

C  MISSILE  JUST  NOW  CLEARING  LAUNCHER 

ELSE 

NCLEAR  =  1 

CALL  OUTMES (0901, T, 0. 0D0) 

UD  =  FX/MASS  +  GB ( 1 ) 

VD  -  FY/MASS  +  GB ( 2 ) 

WD  =  FZ/MASS  +  GB ( 3 ) 

ENDIF 

C  TRANSFORM  BODY  ACCELERATIONS  TO  INERTIAt.  FRAME 


XDD  =  CMI(1)*UD  +  CMIfti'VD  +  CMI(7)*WD 
YDD  «  CMI(2)*UD  +  CMI(5)*VD  +  CMI(8)-WD 
ZDD  *  CMI  (3 )  *(JD  +  CMI(6)*VD  -  CMi(9)*WD 


MXYZDD  =  DSQRT  (  XDD**2  +  YDD**2  *  ZDD’*2  ) 
IF  (  MXYZDD. GT.O.ODO  )  THEN 
*  FTN2 8 6  X418  OPTIMIZE<3> 

99998  CONTINUE 


UXYZDD (1) 

»  XDD  / 

MXYZDD 

UXYZDD (2) 

=  YDD  / 

MXYZDD 

UXYZDD (3) 

=  ZDD  / 

MXYZDD 

ELSE 

UXYZDD (1) 

=  0.0D0 

UXYZDD (2) 

=  0.0D0 

UXYZDD (3) 

=  0.0D0 

ENDIF 

C 


COMPUTE  BODY -TO- INERTIAL  TRANSFORMATION  MATRIX 


n  n  n 


19  Appendix  N:  EXOSIM  2.0  (End-to-end) 


CALL  FV2BXI (QUAT, TMP1 . CMI) 


CTM(l) 
CIM  ( 2 ) 
CIM (3) 
CIM (4) 
CIM ( 5 ) 
CIM (6) 
CIM ( 7 ) 
CIM ( 8 ) 
CIM ( 9) 


CMI (1) 
CMI (4) 
CMI (7) 
CMI (2) 
CMI (5) 
CMI (8) 
CMI (3) 
CMI  (6) 
CMI  (9) 


COMPUTE  EULER  ANGLES 

PHI  =  DBLE ( ATAN2 (CIM ( 8 ) ,CIM(9) ) ) 

THT  =  -DBLE (ASIN  (CIM(7))) 

PSI  =  DBLE (ATAN2 (CIM(4) , CIM (1) ) ) 

TRANSFORM  INERTIAL  VELOCITY  TO  BODY  FRAME 

U  =  CIM(1)*XD  +  CIM(4)*YD  +  CIM(7)*ZD 

V  =  CIM(2)*XD  +  CIM(5)*YD  +  CIM(8)'ZD 

W  =  CIM  ( 3 )  *XD  +  CIM  (  6)  *YD  +  CIM(9!*ZD 

RETURN 

END 


FILE :  uuv22 . 1 9g/dut i 1 i ty /uummk .for 


SUBROUTINE  MMK (A, NA, B, NB, C, NC, RM) 


SUBROUTINE  NAME 


AUTHOR (S)  : 


J.  SHEEHAN 


FUNCTION 


GENERATES  A  DIRECTION  COSINE  MATRIX 
BY  ROTATING  IN  ORDER: 

1)  ANGLE  C  ABOUT  THE  NC  AXIS 

2)  ANGLE  B  ABOUT  THE  NB  AXIS 

3)  ANGLE  A  A30UT  THE  NA  AXIS 


CALLED  FROM 


UTILITY  SUBROUTINE 


SUBROUTINES  CALLED 


ROTMX,  MMLXY 
A,NA,B,NB,C,NC 


OUTPUTS  : 


UPDATES  : 


D.  SMITH  -  CR  »  59 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

DIMENSION  AM  (3, 3)  ,  BM(3,3),  CM(3,3),  RM(3,3),  T(9) 

CALL  ROTMX (A, NA, AM) 

CALL  ROTMX (B, NB, BM) 

CALL  ROTMX (C,NC, CM) 

CALL  MMLXY (BM, CM, T) 

CALL  MMLXY (AM, T, RM) 

RETURN 

END 


FILE:  uuv22 . 1 9g/dut i 1 i ty /uumml xy . for 


SUBROUTINE  MMLXY (X,Y,Z) 


SUBROUTINE  NAME  :  MMLXY 


non 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


c 


AUTHOR (S)  :  J.  SHEEHAN 

FUNCTION  :  MULTIPLY  TWO  3X3  MATRICES 

CALLED  FROM  :  UTILITY  SUBROUTINE 

SUBROUTINES  CALLED  :  NONE 

INPUTS  :  X,  Y 

OUTPUTS  :  Z 


UPDATES  :  D.  SMITH  -  CR  #  59 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 

DIMENSION  X  (3,  3)  ,  Y<3,3),  Z(3,3) 

Z(I,J)  =  X(I,1)*Y(1,J>  +  X(I,2)*Y<2,  J)  +  X  (I,  3)  *Y  (3,  J) 

z(i,D  =  *  xu,2)*Y(2,n  +  xcl,3)*y<3,i) 

Z  (2 . 1 )  =  X  (2. 1)  *Y  (1, 1)  +  X  (2, 2)  *Y  (2,  1)  +  X  1 2,  3)  *  Y  ( 3  .  1 ) 

Z(3,l)  =  X(3,1)*Y(1,1)  +  X  (3, 2)  *Y  (2,  1)  +  X  ( 3.  3 )  *  Y  ( 3 , 1 ) 

2(1,2)  =  X(1,1)*Y(1,2)  +  X(1,2)*Y(2,2)  +  X(1,3)*Y(3,2) 

Z(2,2)  =  X(2,  1)  *Y(1,D  +  X  (2,  2)  *Y  (2,  2)  +  X  (2,  3 )  *  Y  (3,  2 ) 

2(3,2)  *  X  (3, 1)  *Y  (1, 2)  +  X  (3, 2)  *Y  (2, 2)  +  X  ( 3,  3 )  *  Y  (  3 , 2) 

2(1,3)  =  X(1,1)*Y(1,3)  +  X  (1,2)  *Y  (2, 3)  +  X  ( 1 , 3 )  *  Y  ( 3 ,  3 ) 

2(2,3)  =  X(2, 1)  *Y  (1,  3)  +  X  (2, 2)  *Y  (2,  3)  +  X  (2,  3)  *Y  (3,  3) 

2(3,3)  -  X  (3.  1)  *Y  <1, 3)  +  X(3,2)*Y(2,3)  +  X  ( 3,  3  )  *  Y  ( 3  ,  3  ) 

RETURN 

END 


FILE:  uuv22 .19g/dutility/uunavig.for 


C 


C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  NAVIG (T, DELPHI , DELTHT, DELPSI,  DELU, DELV,  DELW, GR, 

QSl.CIE, SP, SQ, SR, SUD, SVD, SWD, VMIR, RMIR, TI2M, 
SPHI, STHT, SPSI, SU, SV, SW, AT, VMI , RMI , TONAV) 


SUBROUTINE  NAME  :  NAVIG 

AUTHOR (S)  :  B.  HILL 

FUNCTION  :  COMPUTES  THE  QUATERNIONS  AND  TRANSFORMATION 

MATRICES  USING  DELTA  ANGLES  SENSED  BY  THE 
GYRO.  COMPUTES  THE  POSITION  AND  VELOCITY  IN 
INERTIAL  AND  EARTH-CENTERED  FRAMES. 

COMPUTES  SENSED  BODY  RATES,  EULER  ANGLES  AND 
THE  GRAVITY-COMPENSATED  ACCELERATION. 

CALLED  FROM  :  FORTRAN  MAIN 

SUBROUTINES  CALLED  :  NONE 

INPUTS  :  T, DELPHI, DELTHT, DELPSI, DELU, DELV, DELW, 

GR, CIE 

OUTPUTS  :  QS1, TI2M, SPHI, STHT, SPSI, SU,SV,SW, AT, VMI, RMI 

BOTH  :  SP, SQ, SR, SUD, SVD, SWD, VMIR, RMIR, TONAV 

UPDATES  :  T.  THORNTON  -  CR  #  0)6 

B.  HILL  -  CR  #  019 

B.  HILL  -  CR  #  022 

B.  HILL  -  CR  *  030 

T.  THORNTON  -  CR  #  033 

T.  THORNTON  -  CR  #  037 

D.  SMITH  -  CR  *  059 

3.  HILL  -  CR  *  062 

D.  SISSOM  -  CR  #  069 

D.  SMITH  -  CR  #  070 

D.  SMITH  -  CR  #  075 

-  -Ur-,.  -  CR  #  076 
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3.  HILl.  /  -  CR  4  081 
R.  RHYNE 

R.  RHYNE  -  CR  it  087 
P.  HILL  -  CR  “  089 
D.  SMITH  -  CR  *  092 
B.  HILL  -  CR  it  093 


IMPLICIT  DOUBLE  PRECISION  (A-K) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

REAL  SP  ,  SQ  ,  SR 

DOUBLE  PRECISION  VMIR<3)  ,  RMIRI3)  ,  VMI ( 3 ) 

DOUBLE  PRECISION  RMI(3) 

REAL  TI  2.M  {  9 ) 

DOUBLE  PRECISION  GR,3> 

DOUBLE  PRECISION  CIE<9) 

REAL  AT (3) 

DOUBLE  PRECISION  QSI (4)  ,  GRAVG ( 3 ) 

DOUBLE  PRECISION  GRLAST ( 3 ) 

C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  INAVIG 

*  DATA  INITIALIZATION 

SINCLUDE  (  1  /  INCLUDE /SSN AVI G  .  DAT  '  ) 

DATA  INAVIG  /  1  / 

IF  (  INAVIG. EQ. I  )  THEN 

INAVIG  =  0 

QS1M  =  DSQRT (ySl (i) *»2  *  QS1<2)**2  -  QS'i3)**Z  -  QSI(-i**2) 

IF  (  QS1M  -EQ.  0.  )  THEN 

C  COMPUTE  QUATERNION  COMPONENTS 

SITHO  =  DSIN {STHT/2 . 0D0) 

COTHO  =  DCOS (STHT/2. 0D0) 

SIPSO  =  DSIN (SPSI/2.0D0) 

COPSO  =  DCOS (SPSI/2 . ODO) 

- -  -  DST*'  (SPE*,"’  000) 

COPHO  =  DCOS (SPHI/2. ODO) 

C  CALCULATE  QUATERNIONS 

QSI  (4)  =  COPSO*COTHO*COPHO  +  STPS0*SITH0*SIPH0 
QSKI)  =  COPSC'COTHO'SIPHO  -  SI?SO*SITHO*CO?HO 
QS1 ( 2 )  =  COPSO *SITK0*CCPH0  *  S IPSO 'COTHO * STPHC 
QS1 111  =  -COPSO* SI THC*SIPHQ  *  SI?SO*COTHO*CCFHC 

C  COMPUTE  TRANSFORMATION  MATRICES 

TI2M11)  -  QS1  (  4 )  *  *  2  +  QS1(1)**2  -  QS1(2>**2  -  QSi(3)**2 
TI2MI2)  =  2. ODO* (QS1 (1) *QS1 (2)  -  QS1 (4 ) *QS1 (3) ) 

TI2M{3)  =  2. ODO* (QSI (1) *QS1 (3)  +  QS1 ( 4 ) *QS1 (2 ) ) 

TI2MI4)  =  2. ODO*  (QSI  (1)  *QS1  (2)  +  QS.1  ( 4  )  *QS1  ( 3 )  ) 

TI2MI5)  *=  QSI ( 4 ) *  *  2  -  QSI  ( 1 )  -*  2  +  QS1<2)**2  -  QS1<3)**2 
TI2MI6)  =  2. ODO* (QSI (2) *QSI (3)  -  QSI ( 4 ) *QS1 ( 1 ) ) 

TI2MI7)  =  2. ODO* (QSI (1) *QS1 (3)  -  QSI ( 4 ) *QSi ( 2 ) ) 

TI2M(8)  =  2. ODO* (QSI (2) *QS1 (3)  *  QSI t 4 } *QS1 (1 ) ) 

TI2M19)  =  QSI  { 4 ) *  *  2  -  QSI ( i ) *  *  2  -  QS1(2)**2  -  QS1(3)**2 

ENDIF 

ENDIF 

DTDEL  =  T  -  TCNAV 
TONAV  -  T 

C  COMPUTE  CORRECTED  INTEGRAL  ANGLES 

DTX  =  0.5C0* DELPHI 
DTY  =  0 . 5D0*DELTHT 
DTZ  =  0.5D0*DEL?ST 

C  INTERMEDIATE  COMPUTATIONS 

PPO  =  DTX  *  *  2  +  DTY*  *  2  *  DTZ**2 


C 

C 

C 

C 

C 

C 

c 


non 
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?P1  »  (  PPO'QTX  »  27'i-ZTZZ  -  DTZ'LTYC  i 

?P2  »  (  PP0-D7Y  *  272*37X0  -  DrX'DTZC  1 

PP 3  =  (  PP0*D7Z  .  DTX'DTYC  -  DTY’DTXD  > 

C  SET  PAS7  VALUES  GE  CORRECTED  INCSEMENI AT  ANGLES  TO  PRESENT 

□TXO  =  DTX 
DTYO  =  D7Y 
DTZO  -  DTZ 

C  UPDATE  CURRENT  VALUES  OF  CORRECTED  INCREMENTAL  ANGLE 

DTX  =  D7X  -  PPi 
DTY  =  DTY  -  PP2 
DTZ  =  072  -  ?P3 

C  CALCULATE  DELTA  QUATERNIONS 

DUM  =  -0 . 5DO*PPO 

PQO  =  DUM*QSI(4)  -  DTX'QSIC)  -  DTY*QSI(2)  -  DTZ*QS:(3) 

PQ1  =  DTX 'QSI ( 4 )  +  DUM'QSIil)  -  DTZ*QSI(2)  -  D7Y*QS1(3) 

PQ2  =  DTY*QS1<4)  -  DTZ'QSKI)  -  DUM'QS’.IC)  -  DTX*QSL(3) 

PQ3  =  DTZ*QSI(4)  <•  DTY'QSI(I)  -  DTX*QSI(2)  *  DUM*Q3I(3) 

C  UPDATE  QUATERNIONS 

QS1 (4)  =  QSI (4)  *  PQO 
QS1  (1)  »  QSI  (1)  *  PQ1 
QSI  (2)  =  QSI  (2)  +  PQ2 
QSI (3)  =  QSI (3)  ♦  PQ3 

C  NORMALIZE  QUATERNIONS 

DQ  »  C.SDO* (l.ODO-QS:  (4) "2-QS1  ' 1 ) ••2-QSI  ( 2 )  • *  2 -QS I  1  3 1  *  *  2 1 

QSI (1)  =  QSI (I) * (1.3DC  -  DQ) 

QSI (2)  =  QSI (2) * (i.CDO  *  DQ) 

QSI  (3)  =  QSI  (3) * (1.0D0  -  DQ) 

QSI  (4)  =  QSI  (4)  '  (1 . 0D0  ♦  DQ) 

C  COMPUTE  TRANSFORMATION  MATRICES 

TI2M ( 1 )  =  QSI 4  4) -  *  2  *  QSI ( 1 ) -  -  2  -  QSI(2)**2  -  QSI  (  3  >  *  *  2 
T I 2M ( 2 )  *  2.0D0* (QSI (1) *QSI (2)  -  QSI { 4 ) -QS 1(3)) 

TI 2M ( 3 )  =  2.0DO* (QSI (l)*QSi (3)  *  QSI ( 4 ) *QS I ( 2 ) ) 

TI2MI4)  *  2.0DC* (QSI (1) *QS1 (2)  *  QSI (4) "QSI (3) ) 

TI2M  (5)  =  QSI  (  4  )  *  *  2  -  QSI  ( 1 )  -  *  2  ♦  QS1(2)**2  -  QSI  (3)**2 
TI 2M ( 6 )  «  2. 0D0* (QSI (2) 'QSI (3)  -  QSI ( 4 ) "QSI ( I ) ) 

TI2MI7)  =  2 . 0D0* (QSI (1) *QS1 (3)  -  QSI (4) *QS1 (2) ) 

TI2MI8)  -  2.0D0* (QSI (2) *QSI (3)  +  QSI ( 4 ) *QSI ( 1 ) ) 

i- 1 2  M  (  9 )  -  QSI  (  4  )  *  ’  2  -  QSI  ( 1 )  *  *  2  -  QSi(2)**2  *  Q  S  L  (  3  )  *  '  2 

C  COMPUTE  SENSED  EULER  ANGLES 

SPHI  =  DBLE (AT AN 2 ( TI 2M ( 8 ) , TI 2M ( 9) ) ) 

STHT  =  -DBLE (ASIN  (TI2M17))) 
jrii  =  03LE  t  ATA.N2  i TI2M ( 4 ,  ,  TI2M ; 1 '  ' 

CALCULATE  SENSED  ANGULAR  RATES  AND  ACCELERATIONS  IN  BODY  FRAME 

IF  (  DTDEL.GT.O.ODQ  )  THEN 
SP  =  DELPHI/DTDEL 

SQ  =  DELTHT/DTDEL 

SR  =  DELPSI /DTDEL 

SUD  -  DELU/DTDEL 

SVD  *  DELV/DTDEL 

SWD  -  DELW/DTDEL 

END  IE 

TRANSFORM  THE  SENSED  BODY  ACCELERATIONS  TO  THE  INERTIAL  FRAME  (  DOES 
NOT  INCLUDE  GRAVITY  ) 

NOTE  AT  =  (SUD, SVD, SWD)  *  TRANSPOSE [ TM2 I j 

AT ( 1 )  =  TI 2M ( 1 ) *  SUD  *  TI2M(2)*SVD  +  TI2M(3)*SWD 

AT ( 2 )  -  TI 2M ( 4 ) *  SUD  *  TI2M(5)*SVD  »  TI2M(6)*SWD 

AT ( 3 )  «  TI2M (7) *SUD  *  TI2M(8)*5VD  *  TI2M(9)*SWD 

TRANSFORM  THE  SENSED  DELTA  VELOCITIES  INTO  INERTIAL  COORDINATES 

UELXD  »  TI2M(i)*DELU  *  1  J.2M  (2)  *0ELV  *  TI2M(3)*DELW 

DELYD  =  T I  2M  (  4  )  *  DEI.U  *  TI2M(5)*DELV  *•  TI  2M  (  6) -DEL.W 

DELZD  =  T I  2M  (  7  )  *  DELU  +  TI2M(8)*DELV  *■  TI2M(9)*DELW 
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GRAVG  ( i  - 
G  R  A  V  G  i  t  i  — 
GRAVG  { .3 )  -- 


3RAVG (  .  )  -  GR  ( . ) 
jKAVG ( 2 )  —  GR  (  c ) 
IRAVG  l  ' J  =  GR ( 3 i 


SAVE  GRAVITY  VECTOR  FOR  USE  ON  NEXT 

G  ?  LA  ST  C  }  -  G  R  (I  ) 

GR  -  AST  { i  -  _-H  (  l  ) 

GRLAS7 ( 3 )  =  GR ( 3  ) 


GRAVITY  COMPENSATE 


COM? UTF.  SENSES  Mil 


“GRAVG ( I ) 
“GRAVG { 2 ) 
•  GRAVG { 3  ) 


RMIRC)  =  RMIRO  +  DTSEL*  ( VM1R  ( 1 )  - 

rm:r(2>  =  rm:r(2)  *■  dtsel*  ivmiro  - 

RMIR  (3)  =  RMIH(3)  *■  DTTEL*  (VMIR  <3 )  - 

VMIRC)  =  VMIR(i)  *  GELXP 

VMIR  (2)  -  VMIR  (2)  ♦  DEI. YD 

VM I R ( 3 )  =  VM IRC)  -  TELZT 

TRANSFORM  SENSED  INERTIAL  VELOCITIES 


SC  =  TI2MC)  “VMIRC)  -  TI2M(4)  “VMI 

SV  -  T12M  (2)  “VMIR  C )  •  TI2MO-7M I 

SW  -  TI2M<3)  •VMIRC)  *  TI2M(6)“VM: 


TRANSECRM  THE  SENSED  INERTIAL 


RM  1(1)  =  CISC)  •RMIRC)  *  C:E(4)“RM:R| 
rm :  {2)  =  cie  (2)  “Rm;r  i : )  *  cieovrmiri 
RMI  <  3 )  =  CIE (3) “RMIR C )  *  CIE  \  t )  “ RM I R < 


VMI  ( I >  -  CIE  C ) -VMIR  C )  * 
VM 1(2)  -  CIE  (2) “VMIR C )  * 

VMI  (  3  )  -  CIE  (3)  “VMIRC)  * 


LIE  {A)  “VMIR < 
CIE (5)  “VMIR  ( 
GIF.  (  6  )  “VMIR  I 


END 


j  jv2?  .  I  Rq  i • ; ?. :  1  :  ?  ’ 


SUBROUT  I  N?I  NORM  [  ST N ,  I  SEED  ,  RUN ; 


SCBROCT I NE  NAME 


AUTHOR (S) 


CA' LET  FROM  : 


SUBROUTINES  CA1 
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DOUBLE.  PRECISION  MN 
INTEGER *4  1 3 EEC 

SAVE  GSFT  ,  I  SET 
SATA  GSET/0.  /,  ISET-'C/ 

SAT  A  ONE  /  i.COC  / 

DATA  TWO  /  2.CD0  / 

IF  A  SPARE  RANDOM  NUMBER  IS  NET  AVAILABLE  FROM  THE  PREVIOUS  PASS 
GENERATE  TWO  NEW  ONES 

IF  (  I3ET.EQ.0  )  THEN' 

GET  TWO  UNIFORM  RANDOM  NUMBERS  WITHIN  THE  SQUARE  EXTENDING 
FROM  -1  TO  1  IN  EACH  DIRECTION 


I 


VI  =  TWO* RANG  <  TSFED)  -  n\’F. 

V2  =  TWO'RANO  CSEED)  -  ONE 

SEE  IF  THEY  ARE  WITHIN  THE  'JNIT 


NOT  ,  TRY  AGAIN  . 


R  «  V1*V1  *  V2*V2 

IF  (  R.GE.CNE  )  GO  TO  1 

PERFORM  BOX-MULLER  TRANSFORMATION  TO 
RANDOM  NUMBERS  .  RETURN  ONE  AND  SAVE 


-  DSQRT  (  -TWO * D LOG ( R ) / R  ) 
=  FAC*V1 

=  MN  *  SD*FAC*V2 
=  i 


PASS  . 

FAC 
GSET 
RDN 
I  SET 


GuNr-RATr.  TWO  GAUSSIAN 
THE  OTHER  FOR  THE  NEXT 


USE  GAUSSIAN  RANDOM  NUMBER  CARRIED  OVER  FROM  PREVIOUS  PASS 

ELSE  IF  <  I SET . EQ . 1  )  THEN 
RDN  =  MN  *  SD'GSET 

I  SET  =  0 

ENDIF 

RETURN 

END 


ujv2 2 . 1 9q/dut i I i zy /uuobtarg . for 


SUBROUTINE  OBTARO  (T,  GRTEST,  RTEST,  VTEST,  TL2  ) 

C  SUBROUTINE  NAME  :  OBTARG 

f* 

C  AUTHOR (S)  :  D .  SISSOM 

C  FUNCTION  :  COMPUTES  THE  ONBOARD  TARGET  ESTIMATE: 

C  CALLED  FROM  :  FORTRAN  MAIN 

C  SUBROUTINES  CALLED  :  NONE 

C  INPUTS  :  T, GRTEST 

C  BOTH  :  RTEST,VTEST,TL2 

C 

C  UPDATES  :  3.  HILL  -  CR  #  030 

C  T.  THORNTON  -  CR  *  045 

C  B.  HILL  -  CR  *  Cbb 

C  D.  SMITH  -  CR  #  DbP 

C  3.  HILL  -  CR  *  C 62 

C  ? .  SISSOM  -  CR  *  069 

C  D.  SMITH  CR  *  C.'U 

C  B.  HI I L  /  -  CP  *  CHI 

C  R.  RHYNE 


UU  L)  U.  VVUUO 
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c 

R. 

RHYNE 

-  CR 

» 

087 

c 

D. 

SISSOM 

-  CR 

« 

091 

c 

3. 

HILL 

-  CR 

# 

093 

c 

c - 

c 

IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

DOUBLE  PRECISION  RTEST  ( 3 )  ,  VTEST (3) 

DOUBLE  PRECISION  GRTEST ( 3 )  ,  GRTPST ( 3 )  ,  GRTAOB ( 3 ) 

DOUBLE  PRECISION  TARPOS(3)  ,  T/"VEL(3) 

INTEGER  FIRST2 

*  DATA  INITIALIZATION 

SINCLUDE ( 1 VINCLUDE/SSOBTARG. DAT’ ) 

S INCLUDE ( *  * /INCLUDE /SSCON 65 . DAT ' ) 

IF  (  FIRST2  -EQ.  1  )  THEN 
FIRST2  «  0 
TL2  =  T 

C  INITIALIZE  ESTIMATED  TARGET  STATES 

DO  45  IAXIS  -  1,  3 

P.TEST  (IAXIS)  =  TARPOS  (IAXIS) 

VTEST (IAXIS)  =  TARVEL (IAXIS) 

45  CONTINUE 

ELSE 

INTEGRATE  TARGET  ACCELERATION  AnD  VELOCITY  USING  AVERAGE 
GRAVITY  VECTOR  OVER  LAST  INTERVAL 

TDELT  =  T  -  TL2 

TL2  =  T 

DO  2  I  =  1, 3 

GRTAOB ( I )  -  0.5D1  *  (  GRTEST ( I )  +  GRTPST(I)  ) 

RTEST (I)  =  RTEST (I)  +  VTEST ( I )* TDELT  + 

0. 5DO-GRTAOB (I) *TDELT*TDELT 
VTEST (I)  =  VTEST (I)  +  GRTAOB ( I ) *TDELT 

2  CONTINUE 
ENDIF 

SAVE  GRAVITY  VECTOR  FOR  USE  ON  NEXT  PASS 

DO  3  I  =  1,3 

GRTPST (I)  =  GRTEST (I) 

3  CONTINUE 

RETURN 
END 


LE:  uuv22.19g/dutil i ty /uuoutmes . for 


SUBROUTINE  jUTMES (N, T, ARG) 
INTEGER  N 

DOUBLE  PRECISION  T, ARG 
INCLUDE ( ' :PFP: INCLUDE/TARGET. FOR' ) 
CHARACTER*80  MESSAGE 


PROGRAM:  MAIN  ( 01 01  . . .  0200  ) 

IF  (  N . EQ .0101  )  THEN 
WRITE (MESSAGE, 0101)  T 
0101  FORMAT (IX, E16. 9, '  1ST  STAGE  SEPARATION') 

GO  TO  99999 
END  IF 

IF  (  N. EQ. 0102  )  THEN 
WRITE (MESSAGE, 0102)  T 

0102  FORMAT (IX, El  6. 9,  '  2ND  STAGE  SEPARATION') 

GO  TO  99999 
END  IF 

IF  (  -I.EQ.0103  )  THEN 
WRI TE (MESSAGE, 0103)  T 

0103  FORMAT (IX, E16 . 9, '  DROP  NOSE  FAIRING  AND  BOOST  ADAPTER') 
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GO  TO  99999 
END  IF 

IF  (  N.EQ.0104  )  THEN 

WRITE (MESSAGE, 0104)  T, ARG 
0104  FORMAT (1X.E16. 9, IX, E16. 9) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0105  )  THEN 

WRITE (MESSAGE, 0105)  T, ARG 
0105  FORMAT  (IX,  E16. 9,  *  MISS  =  ',£16. 9) 

GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  CMPINV  ( 0201  ...  0300 ) 

C 

IF  (  N.EQ.0201  )  THEN 
WRITE (MESSAGE, 0201) 

0201  FORMAT ( 1  MATRIX  SIZE  TOO  LARGE  IN  CMPINV') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  DISCRT  (0301 ...  0400) 

C 

IF  (  N.EQ.0301  )  THEN 
WRITE (MESSAGE, 0301) 

0301  FORMAT ('  SYSTEM  ORDER  TOO  LARGE  IN  DISCRT') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0302  )  THEN 
WRITE (MESSAGE, 0302) 

0302  FORMAT ('  SUITABLE  CONVERGENCE  WAS  NOT  REACHED  IN  DISCRT') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  EIGVEC  (0401  ...  0500) 

C 

IF  (  N.EQ.0401  )  THEN 
WRITE (MESSAGE, 0401) 

0401  FORMAT ( '  MATRIX  SIZE  TOO  LARGE  IN  EIGVEC’) 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  HQR  (0501  ...  0600) 

C 

IF  (  N. EQ .0501  )  THEN 
WRITE (MESSAGE, 0501) 

0501  FORMAT C  TOO  MANY  ITERATIONS  IN  HQR') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  KALMAN  (0601  ...  0700 

IF  (  N . EQ .0601  )  THEN 
WRITE (MESSAGE, 0601 )  T 

0601  FORMAT ( IX, El  6 . 9,  '  INITIATE  ACQUISITION  PHASE’) 
GO  TO  99999 

END  IF 

IF  (  N . EQ .0602  )  THEN 
WRITE (MESSAGE, 0602)  T 

0602  FORMAT ( IX, El  6 . 9,  '  INITIATE  TRACK  PHASE') 

GO  TO  99999 

END  IF 

IF  (  N . EQ .0603  )  -"HEN 
WRITE (MESSAGE, 0603)  T 

0603  FORMAT ( 1 X, El  6 . 9,  '  INITIATE  TERMINAL  PHASE’) 

GO  TO  99999 
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END  IF 

IF  (  N.EQ.0604  )  THEN 

WRITE (MESSAGE, 0604)  T,ARG 

0604  FORMAT ( IX, El  6 . 9,  1  ACQUISITION  MODE  ENABLED:  MAGRO  =  ',E16.9) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0605  )  THEN 

WRITE (MESSAGE, 0605)  T, ARG 

0605  FORMAT  (1a,  E16. 9, '  TRACK  MODE  ENABLED:  MAGRO  =  ',E16.9) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0606  )  THEN 

WRITE (MESSAGE, 0606)  T, ARG 

0606  FORMAT (IX, E16. 9, '  CSO  MODE  ENABLED:  MAGRO  =  *,E16.9) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0607  )  THEN 

WRITE (MESSAGE, 0607)  T, ARG 

0607  FORMAT  (IX,  El  6. 9,  >  TERMINAL  MODE  ENABLED:  MAGRO  =  \E16.9) 

GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  MATINV  (0701  ...  0800) 

C 

IF  (  N.EQ.0701  )  THEN 
WRITE (MESSAGE, 0701) 

0701  FORMAT C  MATRIX  SIZE  TOO  LARGE  IN  MATINV') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  MCGUID  (0801 ...  0900) 

C 

IF  (  N.EQ.0801  )  THEN 
WRITE (MESSAGE, 0801)  T 

0801  FORMAT (IX, E16. 9, '  KV  PITCHOVER  COMPLETE', 

4  '  -  BEGIN  DISTURBANCE  MEASUREMENT') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0802  )  THEN 
WRITE (MESSAGE, 0802)  T 

0802  FORMAT (IX, E16. 9, '  DISTURBANCE  MEASUREMENT  COMPLETE', 
4  '  -  ORIENT  KV  TO  LOS') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0803  )  THEN 
WRITE (MESSAGE, 0803)  T 

0803  FORMAT ( IX, El  6 . 9,  '  KV  ORIENTATION  COMPLETE') 

GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  MISSIL  ( 0 901 . . . 1 000 ) 

C 

IF  !  N.EQ.0901  )  THEN 
WRITE (MESSAGE, 0901)  T 

0901  FORMAT (IX, E16. 9, •  MISSILE  HAS  CLEARED  THE  LAUNCHER') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  OPTSSC  ( 1 001 . . . 1 1 00) 

C 

IF  (  N.EQ.1001  )  THEN 
WRITE (MESSAGE, 1001) 

1001  FORMAT C  MAXIMUM  NUMBER  OF  STATES  EXCEEDED  IN  OPTSSC') 
GO  TO  99999 
END  IF 


nnn  >-■  noo  m  nnn  >->  ^  nnn  ^  on  n 
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SUBROUTINE:  RANO  (1 1 01 . . . 1 200 ) 

IF  (  N.EQ.1101  )  THEN 
WRITE (MESSAGE, 1101 ) 

101  FORMAT ('  RANDOM  NUMBER  OUT  OF  BOUNDS  IN  RANO1) 
GO  TO  99999 
END  IF 


SUBROUTINE:  SEEKER  ( 1201  ...  1 300 ) 

IF  (  N.EQ.1201  )  THEN 
WRITE (MESSAGE, 1201)  T 

201  FORMAT (IX, E16. 9,  '  TRUE  LOS  ANGLE  EXCEEDS  FIELD-OF-VIEW  LIMIT') 
GO  TO  99999 

END  IF 

IF  (  N.EQ.1202  )  THEN 
WRITE (MESSAGE, 1202)  T 

202  FORMAT (IX, E16. 9, '  TARGET  REACQUIRED') 

END  IF 

IF  (  N.EQ.1203  )  THEN 

WRITE (MESSAGE,  1203)  T,  ARG 

203  FORMAT (IX, El  6. 9,  '  FRAME  RATE  CHANGE:  FRMRAT  =  ’,E16.9) 

GO  TO  99999 

END  IF 


SUBROUTINE:  SSPLAG  (1301 ...  1400) 

IF  (  N.EQ.1301  )  THEN 
WRITE (MESSAGE, 1301) 

301  FORMAT ('  BUFFER  SIZE  INSUFFICIENT  IN  SSPLAG’) 
GO  TO  99999 
END  IF 


SUBROUTINE:  TARGET  ( 1  <1 01  .  .  .  1 500 ) 

IF  (  N.EQ.1401  )  THEN 

WRITE (MESSAGE, 1901)  T, ARG 

401  FORMAT  (IX,  E16. 9,  '  TARGET  RESOLVED:  RANGE  =  \E16.9) 
GO  TO  99999 
END  IF 


SUBROUTINE:  VCSLOG  ( 1 501 .  . . 1 600 ) 

IF  (  N.EQ.1S01  )  THEN 

WRITE (MESSAGE, 1501)  T, ARG 

1501  FORMAT (IX, E16. 9, '  ISSUE  MIDCOURSE  DISTURBANCE  BURN', 

S  '  -  VCS  THRUSTER  ',F2.0) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.1502  )  THEN 

WRITE (MESSAGE, 1502)  T, ARG 

1502  FORMAT (IX, E16. 9, '  ISSUE  MIDCOURSE  BURN  ’,F2.0) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1503  )  THEN 

WRITE (MESSAGE, 1503)  T, ARG 

1503  FORMAT (IX, E16. 9, '  ISSUE  MIDCOURSE  BURN  ',F2.0, 

4  '  -  BURN  TIME  BELOW  THRESHOLD') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.1504  )  THEN 
WRITE (MESSAGE, 1504)  T 

1504  FORMAT (IX, E16 . 9, '  ISSUE  FIRST  BURN') 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1505  )  THEN 
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WRITE (MESSAGE, 1505)  T 

1505  FORMAT (IX, E16. 9, 1  ISSUE  FIRST  BURN’, 

£  ■  -  BURN  TIME  BELOW  THRESHOLD’) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1506  )  THEN 
WRITE (MESSAGE,  1506)  T 

1506  FORMAT ( IX, El  6 . 9,  '  ISSUE  SECOND  BURN’) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1507  )  THEN 
WRITE (MESSAGE, 1507)  T 

1507  FORMAT  (IX,  El  6 . 9,  '  ISSUE  SECOND  BURN”, 

£  ’  -  BURN  TIME  BELOW  THRESHOLD’) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1508  )  THEN 
WRITE (MESSAGE, 1508)  T 

1508  FORMAT (IX, El  6. 9,  '  ISSUE  THIRD  BURN’) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1509  )  THEN 
WRITE (MESSAGE,  1509)  T 

1509  FORMAT (IX, E16. 9, ’  ISSUE  THIRD  BURN’, 

£  '  -  BURN  TIME  BELOW  THRESHOLD’) 

GO  TO  99999 

END  IF 


WRITE (MESSAGE, 0001)  N 

0001  FORMAT ( '  ERROR:  MESSAGE  NUMBER  =  ’,14) 


99999  CONTINUE 

CALL  OUTPUT_MESSAGE (  %VAL (CHARACTER  08BIT) ,  MESSAGE  ) 
CALL  OUTPUT_NL 

RETURN 

END 


FILE :  uuv22 . 1 9g/dut i 1 ity /uuran . for 


REAL  FUNCTION  RAN ( I  SEED ) 


SUBROUTINE  NAME  : 

RAN 

AUTHOR  (S)  : 

D.  F.  SMITH 

FUNCTION  : 

GENERATES  A  UNIFORMLY  DISTRIBUTED  RANDOM 
NUMBER 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

SUBROUTINES  CALLED  : 

NONE 

INPUTS  : 

NONE 

OUTPUTS  : 

RAN 

BOTH  : 

ISEED 

UPDATES  : 

NONE 

INTEGER*  4  ISEED 

iseed  =  69069*iseed  +  1 

ran  =  abs ( float ( i seed) /21 4 748364 7 . 0 ) 

RETURN 

END 


no  on 
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FILE :  uuv22 .19g/dutility/uuran0.for 


C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


DOUBLE  PRECISION 

FUNCTION  RAN0 (ISEED) 

SUBROUTINE  NAME  : 

RAN0 

AUTHOR (S)  : 

D.  F.  SMITH 

FUNCTION  : 

GENERATES  A  UNIFORMLY  DISTRIBUTED  RANDOM 
NUMBER  BETWEEN  0  AND  1  USING  THE  SYSTEM 
ROUTINE  RAN (ISEED)  .  THE  BUFFER  IN  COMMON 
BLOCK  RANCOM  IS  INITIALIZED  BY  CALLING 
ROUTINE  RANIT  . 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

SUBROUTINES  CALLE 

D  :  RAN 

INPUTS  : 

NONE 

OUTPUTS  : 

RANG 

BOTH  : 

ISEED 

UPDATES  : 

NONE 

NOTE  :  IMPLICIT  DOUBLE  PRECISION  IS  NOT  NEEDED  SINCE  THE  OUTPUT 
OF  RAN  IS  SINGLE  PRECISION 


INTEGER*  4  ISEED 

COMMON  /  RANCOM  /  RANSEQ (97),  RANLST 

USE  PREVIOUSLY  SAVED  RANDOM  NUMBER  AS  BUFFER  INDEX  AND  MAKE 
SURE  ARRAY  BOUNDS  ARE  NOT  EXCEEDED  . 

J  -  1  +  INT  (  97 . 0*RANLST  ) 

IF  (  J.LT.l  .OR.  J.GT.97  )  THEN 
CALL  OUTMES (1100,0.000,0.000) 

END  IF 

RETRIEVE  RANDOM  NUMBER  FROM  BUFFER  FOR  OUTPUT  AND  SAVE  IT  FOR 
USE  AS  AN  INDEX  ON  THE  NEXT  PASS  . 

RANLST  =  RANSEQ (J) 

RAN0  -  DBLE  <  RANLST  ) 

C  LOAD  A  NEW  RANDOM  NUMBER  IN  THE  SLOT  JUST  VACATED  . 

RANSEQ (J)  =  RAN  (  ISEED  ) 

RETURN 

END 


FILE:  uuv22 . 19g/dutility/uuranit.for 


C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  RANIT  ( 

ISEED  ) 

SUBROUTINE  NAME  : 

RANIT 

AUTHOR (S)  : 

D.  F.  SMITH 

FUNCTION  : 

INITIALIZES  A  TABLE  OF  RANDOM  NUMBERS  FOR 
USE  BY  THE  UNIFORM  RANDOM  GENERATOR  RAN0 

CALLED  FROM  : 

EXECUTIVE  ROUTINE 

SUBROUTINES  CALLED 

:  RAN 

INPUTS  : 

NONE 

OUTPUTS  : 


NONE 
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c 

C  BOTH  :  I  SEED 

C 

C  UPDATES  :  NONE 

C 

c - 

C  NOTE  :  IMPLICIT  DOUBLE  PRECISION  IS  NOT  NEEDED  SINCE  THE  OUTPUT 

C  OF  RAN  IS  SINGLE  PRECISION 

INTEGER*^  RANIT 

COMMON  /  RANCOM  /  RANSEQ (97) ,  RANLST 

C  EXERCISE  SYSTEM  ROUTINE 

DO  10  I  =  1  ,  97 

DUMMY  =  RAN  (  ISEED  ) 

10  CONTINUE 

C  STORE  97  RANDOM  NUK3ERS  IN  BUFFER  (  97  IS  NOT  SPECIAL  ) 

DO  20  I  =  1  ,  97 

RANSEQ (I)  =  RAN  (  ISEED  ) 

20  CONTINUE 

C  SAVE  ANOTHER  RANDOM  NUMBER  TO  USE  FOR  INDEXING  BUFFER 

RANLST  =  RAN  (  ISEED  ) 

RETURN 

END 


FILE :  uuv22 . 19g/dut ility/uurelat .for 


C - 

SUBROUTINE  RELAT (RTIC, VTIC, X, Y, 2, XD, YD, 2D,  Q,  R, CIM, CMS,  RRELTR, 


MAGRTR, VRELTR, MGRDTR, MAGLOS, LAMTRU, LAMDXX , 
LAMDTR, LAMSEK, LAMDSK, TGOTR, RRELM, VRELM, CA2, CEL) 


C--- 

c 

c 

SUBROUTINE  NAME  : 

RELAT 

c 

AUTHOR (S)  : 

T.  THORNTON 

c 

FUNCTION  : 

COMPUTES  RELATIVE  RANGE,  RANGE  RATE, 

c 

TIME-TO-GO, LOS  ANGLES  AND  RATES 

c 

CALLED  FROM  : 

FORTRAN  MAIN 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

INPUTS  : 

RTIC, VTIC,X,Y,2,XD 

,  YD,  ZD,  Q,R,  CIM,  CMS 

c 

OUTPUTS  : 

RRELTR, MAGRTR, VRELTR  MGRDTR, MAGLOS, LAMTRU, 

c 

LAMDXX, LAMDTR, LAMSEK, LAMDSK, TGOTR, RRELM, 

c 

VRELM, CAZ , CEL 

c 

UPDATES  : 

T.  THORNTON  -  CR 

# 

037 

c 

B.  HILL  -  CR 

# 

038 

c 

T.  THORNTON  -  CR 

» 

048 

c 

D.  SMITH  -  CR 

» 

059 

c 

B.  HILL  /  -  CR 

» 

081 

c 

R.  RHYNE 

c 

D.  SISSOM  -  CR 

* 

091 

c 

B.  HILL  -  CR 

* 

093 

c 

c - 

IMPLICIT  DOUBLE  PRECISION  (A-H) 
IMPLICIT  DOUBLE  PRECISION  (0-2) 


REAL 

CIM  ( 9) 

DOUBLE 

PRECISION 

CMS (9) 

,  MAGLOS 

DOUBLE 

PRECISION 

RTIC (5, 3) 

REAL 

RRELTR (3) 

,  URRELT ( 3 ) 

REAL 

MAGRTR 

DOUBLE 

PRECISION 

VTIC (5, 3) 

REAL 

VRELTR (3) 

,  MAGVTR 

,  VRDRRT 

DOUBLE 

PRECISION 

MGRDTR 

,  RRELM (3) 

o  o 


110  Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1 ,  Part  3 

DOUBLE  PRECISION  VRELM ( 3 )  ,  LAMTRU (2 )  ,  LAMDXX(2) 

DOUBLE  PRECISION  LAMDTR ( 2 )  ,  RRELS ( 3 )  ,  VRELS(3) 

REAL  LAMSEK ( 2 )  ,  Q  ,  R 

REAL  TGOTR 

DOUBLE  PRECISION  LAMDSK  (2)  ,  CA2U00) 

DOUBLE  PRECISION  CEL(IOO) 

INTEGER  St-KTY? 

*  DATA  INITIALIZATION 
$  INCLUDE  { •  A  'TNCLUDE/SSCON50  .  DAT  •  > 

$ INCLUDE ( 1 ** / INCLUDE/SSCON66 . DAT ' ) 

SINCLUDE ( ' A /INCLUDE/ SSCON21 . DAT ' ) 

TF  (  SEKTYP  .EQ.  3  )  THEN 
DO  65  IOBJ  =  1,  NOBJ 

CALCULATE  TEMPORARY  RELATIVE  RANGE  FOR  EACH  OBJECT 
IN  INERTIAL  FRAME 

TMP1  -  RTIC(IOBJ,l)  -  X 

TMP2  =  RTIC(IOBJ,2)  -  Y 

TMP3  =  RTIC (IOBJ, 3)  -  Z 

C  TRANSFORM  TO  MISSILE  FRAME 

TMP4  =  TMP1 *CIM ( 1 )  +  TMP2*CIM(4)  +  TMP3*CIM(7) 

TMP5  ■  TMP1*CIM<2)  +  TMP2*CIM(5)  +  TMP3*CIM(8) 

TMP  6  =  TMP1 *CIM ( 3 )  +  TMP2*CIM(6)  +  TMP3*CIM(9) 

C  TRANSFORM  TO  SEEKER  FRAME 

TMP 7  =  TMP 4 ‘CMS ( 1 )  *  TMP5*CMS(4)  +  TMP6*CMS(7> 

TMP 8  =  TMP4*CMS(2)  +  TMPS*CMS(5)  +  TMP6*CMS(8) 

TMP 9  =  TMP4*CMS(3)  +  TMP5*CMS(6)  +  TMP6*CMS(9) 

C  DETERMINE  ELEVATION  AND  AZIMUTH  FOR  EACH  OBJECT 

CEL ( IOBJ)  «  DATAN2 (-TMP9, TMP7) 

CAZ (IOBJ)  =  DATAN2 (  TMP8 , TMP7 ) 

65  CONTINUE 
ENDIF 

C  COMPUTE  RELATIVE  RANGE,  RANGE  RATE,  AND  TIME-TO-GO 

RRELTR ( 1 )  =  RTIC (1,1)  -  X 
RRELTR  ( 2 )  -  RTICfl,  2)  -  Y 
RRELTR  ( 3 )  =  RTIC  (1,3)  -  Z 

MAGRTR  =  SORT (RRELTR (1) ** 2  +  RRELTR(2)**2  +  RRELTR ( 3) *  *  2 ) 

URRELT ( 1 )  -  RRELTR (1) /MAGRTR 
URRELT (2)  =  RRELTR (2) /MAGRTR 
URRELT (3)  =  RRELTR (3) /MAGRTR 

VRELTR ( 1 )  =  VTIC (1,1)  -  XD 
VRELTR ( 2 )  =  VTIC (1,2)  -  YD 
VRELTR ( 3 )  =  VTIC (1,3)  -  ZD 

MAGVTR  =  SQRT (VRELTR (1) **2  +  VRELTR  (2)  “2  +  VRELTR ( 3 ) *  * 2 ) 

MGRDTR  =  VRELTR (1) ‘URRELT (1)  +  VRELTR (2) ‘URRELT (2)  + 

VRELTR ( 3 ) ‘URRELT ( 3 ) 

VRDRRT  =  VRELTR (1) ‘RRELTR (1)  +  VRELTR (2 ) ‘RRELTR ( 2 )  + 

VRELTR (3) ‘RRELTR (3) 

TGOTR  -  -VRDRRT/ (MAGVTR** 2) 

C  COMPUTE  LOS  ANGLES  AND  RATES  IN  BODY  FRAME 

RRELM(l)  =  RRELTR ( 1 ) *CIM ( 1 )  +  RRELTR (2 ) *CIM ( 4 )  +  RRELTR ( 3 ) *CIM ( 7 ) 

RRELM ( 2 )  =  RRELTR ( 1 ) *CIM ( 2 )  +  RRELTR (2) *CIM (5)  +  RRELTR ( 3 ) *CIM ( 8 ) 

RRELM ( 3 )  =  RRELTR (1) *CIM (3)  +  RRELTR ( 2 ) *CIM ( 6)  +  RRELTR ( 3 ) *CIM ( 9) 

VRELM (1 )  =  VRELTR ( 1 ) *CIM ( 1 )  +  VRELTR ( 2 ) *CIM ! 4 )  +  VRELTR ( 3 ) ‘Cl”  ’ - > 

VRELM ( 2 )  -  VRELTR ( 1 ) *CIM ( 2 )  +  VRELTR ( 2 ) *CIM ( 5 )  *  VRELTR ( 3 ) *CIM ( 8 ) 

VRELM ( 3 )  «  VRELTR (1) *CIM (3)  +  VRELTR (2 ) *CIM ( 6)  +  VRELTR ( 3 ) *CIM ( 9 ) 

LAMTRU ( 1 )  =  DAT AN 2 ( -RRELM ( 3 ) , RRELM ( 1 ) ) 

LAMTRU ( 2 )  =  DATAN2 (RRELM (2) , RRELM (1) ) 

LAMDXX ( 1 )  =  (RRELM ( 3 ) ‘VRELM (1 )  -  RRELM (1 ) ‘VRELM ( 3 ) )  / 

(RRELM  ( 1 )  “2  ♦  RRELM  ( 3 !  *  * 2 ) 
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LAMDXX (2) 

LAMDTR ( 1 ) 
LAMDTR (2) 


(RRELM  (1 )  ’VRELM  (2  )  -  RREL.M  ( 2 )  *  VRELM  ( 1 )  )  / 
(RRELM ( 1 ) *  *2  +  RRELM(2)”2) 

LAMDXX (1)  -  Q 
LAMDXX (2)  -  R 


C  COMPUTE  LOS  ANGLES  AND  RATES  IN  SEEKER  FRAME 


RRELS ( 1 ) 
RRELS (2) 
RRELS (3) 


RRELM (1) *CMS (1)  + 
RRELM ( 1 ) "CMS ( 2 )  + 
RRELM (1) *CMS (3)  + 


RRELM (2) «CMS<4> 
RRELM (2) *CMS<5) 
RRELM(2)*CMS(6) 


RktLM (3) *CMS (7) 
RRELM (3) *CMS (8) 
RRELM (3) *CMS (9) 


VRELS (1) 
VRELS (2) 
VRELS (3) 


VRELM (1) ’CMS (1)  + 
VRELM (1) ’CMS (2)  + 
VRELM (1) *CMS (3)  + 


VRELM (2) ’CMS (4) 
VRELM (2) ’CMS (5) 
VRELM ( 2 ) *CMS (6) 


+  VRELM (3) *CMS (7) 
+  VRELM (3) *CMS (8) 
+  VRELM (3) ’CMS (9) 


LAMSEK ( 1 ) 
LAMSEK  (2) 
MAGLOS 

LAMDSK(l) 

LAMDSK (2) 


SNGL  (  DATAN2 (-RRELS (3) , RRELS (1) )  ) 

SNGL  (  DATAN2 (RRELS (2) , RRELS (1) )  ) 

DABS  (DATAN2  (DSQRT  (RRELS  (2)  ”2  +  RRELS ( 7 )  , 

RRELS (1) ) ) /DTR 

(RRELS (3) ’VRELS (1>  -  RRELS ( 1 ) "VRELS ( 3 ) )  / 
(RRELS ( 1 ) ”2  +  RRELS (3) *  *2) 

(RRELS (1) *VRELS (2)  -  RRELS (2: ’VRELS (1 ) )  / 
(RRELS (1) ”2  +  RRELS (2 ) * "2 ) 


RETURN 

END 


FILE :  uuv22 . 1 9g/dut il ity /uuresp2r . for 


SUBROUTINE  RESP2R  ( 

DT,WD,  ZD,  CILL,  Cl L,  Cl,  COLL,  COL,  CO  ) 

SUBROUTINE  NAME  : 

RESP2R 

AUTHOR (S)  : 

D.  F.  SMITH 

FUNCTION  : 

Given  a  second  order  continuous  filter  of 
the  form 

WD**2 

G(s)  - - - 

s**2  +  2.0*ZD*WD*s  +  WD**2 

compute  a  digital  filter  which  yields  the 
same  ramp  response  .  The  digital  filter  has 
the  transfer  function 

CI*z**2  +  CIL*Z  +  CILL 

G(z>  - - 

CO* z’ * 2  m  CCL* z  +  COLL 

CALLED  FROM  : 

UTILITY  ROUTINE 

SUBROUTINES  CALLED 

:  NONE 

INPUTS  : 

DT# WD, ZD 

OUTPUTS  : 

CILL,  CIL,  CT  .  COLL,  COL,  CO 

UPDATES  : 

NONE 

IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

DAT ft  ONE  /  l.ODO  / 

DATA  TWO  /  2.0D0  / 


C 


Underdamped  filter 
IF  (  ZD.LT.ONE  )  THEN 


A 

= 

WD*  ZD 

B 

= 

WD'DSQRT  (  ONE 

TMP 1 

= 

DEXP  ( 

-  A*DT 

TMP2 

- 

DEXP  ( 

-  TWO*A 

TMP  3 

DCOS  ( 

B*DT  ) 

TMP  4 

= 

DSIN  ( 

B*DT  ) 
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TMP5 
TMP  6 
Cl 
CIL 
CILL 
CO 
COL 
COLL 
END  IF 


A'A  +  B'B 

TMP 1  *  TMP  4  *  {  A'A  -  B'B  ) /B 

TMP5*DT  -  TWO'A  '  TWO'A'TMPl  'TMP 3  +  TMP 6 

TWO' (  A  -  DT*TMP1'TMP3'TMP5  -  TMP 6  -  A'TMP2  ) 

TMP  6  -  TWO'A'TMPl *  TMP 3  +  ?MP2'(  TWO'A  +  TMP5'DT  ) 
TMP5’DT 

TWO * TMP 1* TMP 3* CO 
TMP2*C0 


Critically  damped  filter 
IF  (  ZD.EQ.ONE  )  THEN 


A 

= 

WD 

TMP1 

= 

DEXP  (  -  A* 

DT  ) 

TMP  2 

= 

DEXP  (  -  TWC*A*DT  ) 

TMP  3 

= 

TWO  +■  A'DT 

TMP4 

- 

-  TWO  +  A'DT 

Cl 

* 

TMP1*TMP3  + 

TMP  4 

CIL 

= 

TWO' (  ONE  - 

TWO'A'DT* 

CILL 

TMP1  *TMP  4  + 

TMP2*TMP3 

CO 

= 

A'DT 

COL 

= 

-  CO'TWO'TMPl 

COLL 

= 

CO'TMP2 

TMP  2  ) 


C  Overdamped  filter 


IF  (  ZD . GT .ONE  )  THEN 


TMP  5 

= 

DSQRT  (  ZD" 2  -  ONE  ) 

A 

= 

WD'TMPS 

B 

= 

WD/TMPS 

ASQ 

= 

A'A 

BSQ 

= 

B'B 

EXPA 

= 

DEXP  (  -  A'DT  ) 

EXPB 

= 

DEXP  (  -  B'DT  ) 

TMP1 

= 

A'DT  +  EXPA  -  ONE 

TMP2 

* 

B'DT  +  EXPB  -  ONE 

TMP  3 

= 

ONE  +  A'DT 

TMP  4 

= 

ONE  +  B'DT 

Cl 

- 

ASQ'TMP2  -  BSQ'TMPl 

CIL 

= 

ASQ* (  ONE  -  EXPA*TMP2 
-  BSQ' (  ONE  -  EXPB'TMPl 

CILL 

ASQ'EXPA* (  EXPB*TMP4 
-  BSQ'EXPB* (  EXPA* TMP 3 

CO 

= 

A'B'DT* (  A  -  B  ) 

COL 

= 

-  CO' (  EXPA  +  EXPB  ) 

COLL 

1  IF 

- 

CO'EXPA'EXPB 

■  EX?8*TMP4  ) 
EXPA*  TMP  3  ) 
ONE  ) 

ONE  ) 


RETURN 

END 


FILE:  uuv22.19g/dutility/uuresthr.for 


C 


C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  RESTHR (T, IDIST, ANVP, DTSAMP , TOFLTM, TRATON, TPATON , TYATON , 
DTACSA, DTACSB) 


SUBROUTINE  NAME  : 

RESTHR 

AUTHOR (S)  : 

T.  THORNTON 

FUNCTION 

ATTITUDE  CONTROL  SYSTEM  THRUSTER 
CROSS  COUPLING  LOGIC 

CALLED  FP^M  : 

FORTRAN  MAIN 

SUBROUTINES  CALLED  : 

NONE 

INPUTS  : 

T, IDIST, ANVP, DTSAMP, TOFLTM 

OUTPUTS  : 

DTACSA, DTACSB 

BOTtl  : 

TRATON, TPATON, TYATON 

UPDATES  : 

B.  HILL  -  CR  #  038 

T.  THORNTON  -  CR  *  043 

o  n 
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c 

T. 

THORNTON 

-  CR 

* 

044 

c 

B. 

HILL 

-  CR 

* 

051 

c 

D. 

SMITH 

-  CR 

# 

059 

c 

B. 

HILL  / 

-  CR 

# 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

-  CR 

# 

084 

c 

B. 

HILL 

-  CR 

* 

086 

c 

B. 

HILL 

-  CR 

* 

093 

C 

c 


IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 

REAL  DTACSA  ( 4 )  ,  DTACSBM) 

*  DATA  INITIALIZATION 

S INCLUDE  (  ■  'VINCLUDE/SSCON67.DAT1  ! 

$  INCLUDE  (  •  ''/INCLUDE/ SSCONQ  3  .  DAT  1  ) 

S  INCLUDE  (  1  -'/INCLUDE /SSCONO  8.  DAT'  ) 

C  IN  DISTURBANCE  MOuE  TURN  OFF  ACS  THRUSTERS  WITH  DIVERT  THRUSTERS 

IF  <  IDIST  .EQ.  1  )  THEN 
TMP1  -  TOFLTM  -  T 
IF (  TMP1  .LE.  0.  )  THEN 
TMP2  =  0. 

ELSEIF (  TMP1  . LT .  TSMPH  )  THEN 
TMP2  -  TMP1/TSMPH 
ELSE 

TMP2  =  1. 

ENDIF 

TPATON  =  TPATON*TMP2 
TYATON  =  TYATON*TMP2 
TRATON  =  TRATON*TMP2 
ENDIF 

C  TEST  SIGNS  OF  PITCH,  YAW,  ROLL  AND  ATTITUDE  THRUSTER  PULSEWIDTHS 

C  PITCH  SIGN  TEST 

IF (  TPATON  .GE.  0.0  )  THEN 

*  FTN286  X415  OPTIMIZEO) 

99999  CONTINUE 

TPATP  =  TPATON 
TPATN  =0.0 
ELSE 

TPATP  =0.0 
TPATN  =  -TPATON 
ENDIF 

C  YAW  SIGN  TEST 

IF  (  TYATON  .GE.  0.0  ,  THEN 

*  FTN286  X4 1  5  OPTIMIZEO) 

99998  CONTINUE 

TYATP  =  TYATON 
TYATN  =0.0 
ELSE 

TYATP  =  0.0 
TYATN  =  -TYATON 
ENDIF 

C  ROLL  SIGN  TEST 

IF (  TRATON  .GE.  0.0  )  THEN 

*  FTN286  X415  OPTIMIZEO) 

99997  CONTINUE 

TRATP  =  TRATON 
TRATN  =  0.0 
ELSE 

TRATP  =0.0 
TRATN  =  -TRATON 
ENDIF 

RESOLVE  PITCH,  YAW,  AND  ROLL  THRUSTER  PULSEWIDTHS  INTO 
INDIVIDUAL  THRUSTER  PULSEWIDTHS 

IF (  ANVP  .LT.  1.5  )  THEN 

DTACSA U)  =  TPATP  +  TRATP 
DTACSB(l)  =  TPATN  +  TRATN 
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DTACSA ( 2 )  =  TYATP 

DTACSB ( 2 )  =  TYATN 

DTACSA (3)  =  TPATN  »  TPATP 

DTACSB (3)  =  TPATP  +  TRATN 

DTACSA (4)  *  TYATN 

DTACSB (4)  =  TYATP 

ELSE 

DTACSA (1)  =  TPATP  +  TRATP 

DTACSB (1 )  =  TPATN  +  TRATN 

DTACSA (2)  =  TYATP  +  TRATP 

DTACSB (2)  =  TYATN  +  TRATN 

DTACSA (3)  =  TPATN  +  TRATP 

DTACSB (3)  =  TPATP  +  TRATN 

DTACSA <4 )  -  TYATN  +  TRATP 

DTACSB (4)  -  TYATP  +  TRATN 

ENDIF 

DO  50  1=1, 4 

C  ENFORCE  THRUSTER  PAIR  DEADBANDS 

IF (  ABS(  DTACSA ( I )  -  DTACSB(I)  )  . LT .  ACSDB  )  THEN 

DTACSA (I)  =0.0 
DTACSB (I!  =0.0 
ENDIF 

C  ENFORCE  MINIMUM  COMMAND  ON  TIME 

IF ( (DTACSA ( I )  .LT.  TCMINA  .AND.  DTACSA ( I )  .GT.  0.)  .OR. 

(DTACSB ( I )  .LT.  TCMINA  .AND.  DTACSB (I )  .GT.  0.))  THEN 

*  FTN286  X41 5  OPTIMIZED) 

99996  CONTINUE 

DTACSA (I)  =  DTACSA (I)  *  TCMINA 
DTACSB ( I )  =  DTACSB  ( I )  t  TCMINA 
ENDIF 

IF (  DTACSA ( I )  .GT.  DTSAMP  )  DTACSA ( I )  =  DTSAMP 

IF (  DTACSB  (I)  .GT.  DTSAMP  )  DTACSB (I)  =  DTSAMP 

50  CONTINUE 

RETURN 

END 


FILE :  uuv22 .19g/dutility/uuratmx. for 


C 

C' 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  ROTMX(X,I 

I,  XM) 

SUBROUTINE  NAME  : 

ROTMX 

AUTHOR (S)  : 

J.  SHEEHAN 

FUNCTION  : 

GENERATES  A  DIRECTION  COSINE  MATRIX 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

SUBROUTINES  CALLED  : 

NONE 

INPUTS  : 

X,  I 

OUTPUTS  : 

XM 

UPDATES  : 

D.  SMITH  -  CR  #  59 

IMPLICIT  REAL  (/  H) 
IMPLICIT  REAL  (O-Z) 
REAL  XM  (3,  3) 

SX  «  SIN  (X) 

CX  =  COS (X) 


(  I. 

■  EQ.l 

) 

THEN 

XM  (1 , 

,  1)  = 

1  . 

.  C 

XM  ( 1 , 

,'>)  = 

0  , 

.0 

XM  ( 1 , 

,3)  = 

0. 

.0 

uuu 
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XM(2, 1) 

= 

0.0 

XM (2, 2) 

= 

cx 

XM (2, 3) 

= 

sx 

XM (3, 1) 

0.0 

XM (3, 2) 

= 

-sx 

XM  (3,  3) 

- 

cx 

END  IF 

IF  (  I.EQ. 

.2 

)  THEN 

XM  (1,1) 

= 

CX 

XM  (1.2) 

= 

0.0 

XM(1.  3) 

= 

-SX 

XM (2, 1) 

- 

0.0 

XM (2, 2) 

= 

I .  c 

XM (2, 3) 

= 

2  .  C 

XM  (3,1) 

- 

SX 

XM (3, 2) 

= 

0.0 

XM (3, 3) 

= 

CX 

END  IF 

IF  (  I. EC. 

.  3 

/  i  i  i  i* 

XM ( 1 , 1 ) 

= 

CX 

XM  (1,2) 

= 

sx 

XM(1,  3) 

= 

0.0 

XM  (2,1) 

= 

-sx 

XM ( 2 , 2 ) 

= 

CX 

XM ( 2 , 3 ) 

= 

0.0 

XM(3,  1) 

= 

n .  o 

XM (3, 2) 

= 

0.0 

XM ( 3 , 3) 

= 

1 . 0 

END  IF 

RETURN 

END 

FILE:  i;uv22. 19g/duti  lit.  y/uu  table,  for 


SUBROUTINE  TABLE (XTAB, 

YTAB, X, Y, N,  I) 

SUBROUTINE  NAME  : 

TABLE 

AUTHOR (S)  : 

D .  SMITH 

FUNCTION  : 

PERFORMS  TABLE  LOOKUP  VIA  EITHER  INDEXED 
SEARCH  OR  BINARY  SEARCH  AND  LINEARLY 
INTERPOLATES 

CALLED  FROM  : 

UTILITY  SUBROUT I! 

\.‘E 

SUBROUTINES  CALLED  : 

NONE 

INPUTS  : 

XTAB, YTAB, X, N 

OUTPUTS  : 

V 

BOTH  : 

UPDATES  : 

D .  SMITH  -  CR 
B.  HILL  -  CR 
B .  HILL  -  CR 
D .  SMITH  -  CR 

1  2  ' 

#  JH 

#  46 

#  59 

IMPLICIT  REAL  <A-H> 
IMPLICIT  REAL  (0-2) 
INTEGER  N, I 
REAL  XTAB { N ) , Y  TAB  \  N ) 


IF  (  I.GE.l  .AND.  I.LE-N  )  THEN 
IF  {  X.LE.XTABC)  )  THEN 
Y  -  YTAB ( 1 ) 


o  n  n 
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ELSE  IF'  (  X.GE.XTAB(N)  )  TEEN 

Y  =  YTAB(N) 

I  =  N 

ELSE  IF  (  X.GE.XTAB(I)  I  THEN 
DO  10  K  =  I  ,  N-l 

IF  (  X . LT. XTAB (K+l )  )  GC  TO  20 

CONTINUE 

TRACT  =  (  X  -  XTAB (K)  )  /  (  XTABtK-l)  -  XTAB (K)  I 

Y  =  YTAB(K)  »  TRACT  *  (  YTABIK^l)  -  YTAB(K)  ) 

I  =  K 

ELSE  IF  (  X.LT.XTAB(I)  )  THEM 
DO  30  K  =  1-1  ,  1  ,  -1 

IF  (  X.GE.XTAB(K)  )  GO  TO  40 
CONTINUE 

FRACT  =  (  X  -  XTAB  (K)  I  /  (  X7A3(K*1)  -  XTAB(K)  ) 

Y  =  YTAB(K)  -  FRACT  *  (  YTA3(X*I)  -  YTAB (X)  ) 

I  =  K 

END  IF 

PERFORM  BINARY  SEARCH  IF  POINTER  IS  ZERO  OR  OUT  OF  BOUNDS 

ELSE  IF  (  I.LT.l  .OR.  I . GT . N  )  THEN 

IF  {  X  .  GT .  XTAB  ( I )  .AND.  X  .  LT  .  XTAB  ( N )  )  THEN 

K  =  1 

L  =  N 

DO  SO  I  =  K  ,  L 

IF  (  L.EQ.K+1  i  GO  TO  60 
M  =  (  K  +  L  )/  2 

IF  I  X.LT.XTAB(M)  )  THEN 
L  =  M 

ELSE 

K  =  M 

END  IF 
CONTINUE 

FRACT  =  (  X  -  XTAB ( K )  )  /  (  XTAB ( L)  -  XTA3IK)  ) 

Y  =  YTAB(K)  -  FRACT  *  (  YTAB (L)  -  YTAB (X)  ) 

I  =  K 

ELSE  IF  (  X . LE . XTAB ( 1 )  )  THEN 

Y  =  YTAB ( 1 ) 

I  =  1 

ELSE  IF  {  X.GE.XTAB(N)  )  THEN 

Y  =  YTAB (N) 

I  =  N 

END  IF 
END  IF 

RETURN 

END 


FILE:  uuv22 . 1 9q/dut i I i ty /uuta rqes . for 


SUBROUTINE  TARGET (  T, MAGRTR, CAZ, CEL, CER, CIE, PTARG, QTARG, RTARG, 
TPHI, TTHT, TPS I ,GRT, TPHID, TTHTD, TPSID,CIT, 
RTIC, VTIC, RTAR, RTER, NSUB, IRESLV,  RJ,  CTI, 
VTAR, LATT, LONGT, AZSUB,  EL3UB,  RJSUB  ) 


SUBROUTINE  NAME  : 


AUTHOR (S)  : 


FUNCTION 


IALLED  FROM 


JBROUTINES  CALLEI 


COMPUTES  THE  ROTATIONAL  AND  TRANSLATIONAL 
STATES  FOR  EACH  OBJECT 


CRT RAN  MAIN 


T,  MAGRTR,  CAZ,  CEL.  CER,  Cl?.,  PTARG,  CTARG,  RTARG 

TPHI , TTHT, TPSI , GRT, TPHID, TTHTD. TPS I D, Cl T. 
RTIC, VTIC, RTAR, RTER, NSUB, IRESLV 

RJ, CTI , V TAR, LATT, LONGT, AZSUB, EL5U3, RJSUB 

3.  HILL  -  UR  <  331. 

T.  THDRST CN  -  UR  *  145 
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c 

T . 

THORNTON 

-  CR 

* 

04” 

c 

B. 

HILL 

-  CR 

* 

055 

c 

D. 

SMITH 

-  CR 

« 

059 

c 

B. 

HILL 

-  r  r 

» 

0b2 

c 

D. 

SISSOM 

- 

# 

069 

c 

D. 

SMITH 

-  CR 

» 

070 

c 

B. 

HILL  / 

-  CR 

» 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

-  CR 

« 

087 

c 

D. 

SISSOM 

-  CR 

* 

091 

c 

B. 

HILL 

-  CR 

* 

093 

c 

D. 

SISSOM 

-  CR 

» 

094 

c 

c 


IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (C-Z) 

CHARACTER*128  MESSAGE 


DOUBLE 

PRECISION 

AZSUB (100) 

,  CAZ (100) 

,  CEL(iOO) 

REAL 

CER  (9) 

DOUBLE 

PRECISION 

CIE (9) 

REAL 

CIT (9) 

DOUBLE 

PRECISION 

CLTPOS  (3) 

,  CLTVEL ( 3 ) 

,  CSOPOS (3) 

DOUBLE 

PRECISION 

CSOVEL  (3) 

,  CTI (9) 

,  ELSUB  (100) 

DOUBLE 

PRECISION 

GRT (5, 3) 

DOUBLE 

PRECISION 

GRTAVG  (5,3) 

,  GRTLST  (5,3) 

DOUBLE 

PRECISION 

LATT 

,  LONGT 

REAL 

MAGRTR 

DOUBLE 

PRECISION 

MGRT 

,  MRTIC 

,  RHOPOS ( 3 ) 

DOUBLE 

PRECISION 

RHOVEL (3) 

,  RJ (5) 

,  RJSUB(IOO) 

DOUBLE 

PRECISION 

RTAR (3) 

,  RTER ( 3 ) 

,  RTIC  (5,3) 

DOUBLE 

PRECISION 

TARPOS (3) 

,  TARVEL (3) 

,  TNKPOS (3) 

DOUBLE 

PRECISION 

TNKVEL (3) 

,  URTIC ( 3 ) 

DOUBLE 

PRECISION 

VTAR (3) 

,  VTIC  (5,3) 

INTEGER  FIRST1 

INTEGER  SEKTYP 


*  DATA  INITIALIZATION 
SINCLUDE  ( '  "VINCLUDE/SSTARGET.  DAT  • ) 
SINCLUDE ( 1 A /INCLUDE /SSCON21 . DAT  * ) 
SINCLUDE  (  1 /  INCLUDE/ SSCON3 9  .  DAT  ‘  ) 
SINCLUDE  (  1 'VINCLUDE/SSCON50.DAT'  ) 
SINCLUDE  < • VINCLUDE/SSCON65.DAT* ) 
SINCLUDE ( 1  * /INCLUDE/SSCON66 . DAT  1 ) 
SINCLUDE  (  1  /'/INCLUDE/SSCON69.DAT‘  ) 

IF  (  FIRST1  .EQ.  1  )  THEN 
FIRST1  =  0 
TL1  =  T 


C  INITIALIZE  STATES  FOR  EACH  OBJECT 


DO  45  IAXIS  «  1, 
RTIC (1, IAXIS) 
RTIC (2, IAXIS) 
RTIC (3, IAXIS) 
RTIC (4, IAXIS) 
RTIC (5, IAXIS) 
VTICU,  IAXIS) 
VTIC (2, IAXIS) 
VTIC (3, IAXIS) 
VTIC (4, IAXIS) 
VTIC (5, IAXIS) 
45  CONTINUE 

RJ ( 1 )  =  TARRI 
R J ( 2 )  =  CSORI 
RJ ( 3 )  *  TNKRI 
R J ( 4 )  =  RHORI 
RJ ( 5 )  =  CLTRI 
ENDIF 

IF  (  SEKTYP  . EQ.  3 


3 

=  TARPOS (IAXIS) 
=  TARPOS (IAXIS) 
=  TARPOS (IAXIS) 
=  TARPOS (IAXIS) 
=  TARPOS (IAXIS) 
=  TARVEL (IAXIS) 
*  TARVEL (IAXIS) 
=  TARVEL (IAXIS) 
=  TARVEL (IAXIS) 
=  TARVEL (IAXIS) 


THEN 


+  CSOPOS (IAXIS) 
+  TNKPOS (IAXIS) 
+  RKOPOS (IAXIS) 

*  CLTPOS (IAXIS) 

+  CSOVEL ( IAXIS) 

♦  TNKVEL ( IAXIS) 
+  RHOVEL (IAXIS) 
+  CLTVELI IAXIS! 


C  DETERMINE  IF  TARGET  IS  LARGER  THAN  A  USER-INPUT 

C  MULTIPLE  OF  A  PIXEL  FIELD  OF  VIEW 


IF  (  MAGRTR  .GT.  0.0  .AND.  NTARRS  . EQ .  I  )  THEN 
*  FTN286  X415  OPTIMIZEI3) 

99999  CONTINUE 

IF  (  DMAX1  (TARWID,  TARLEN) /MAGRTR  .  GF. . 


on  o  non 
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RMULT* (WIDTH/FOCLEN)  )  THEN 
IF  (  IRESLV  .£Q.  0  )  THEN 
IRESLV  *  1 

DELTL  =  (1.0/12.0) *TARLEN 
DELTW  =  (1.0/11.0) *TARWID 
RJNEW  =  (1.0/13.0) *TARRI 
CALL  OUTMES <1401, T, DBLE (MAGRTR) ) 

ENDIF 

GENERATE  RESOLVABLE  TARGET  (ON  FIRST  PASS  ONLY)  - 
MODEL  THIS  TARGET  AS  SUPERPOSITION  OF  MANY  OBJECTS 
WITH  CENTROID  AT  ILEN=0,  IWID=0 

NSUB  =  0 

DO  5  ILEN  »  -8,  4 

IF  (  ILEN  .EQ.  -8  .OR.  ILEN  .EQ.  -7 
•OR.  ILEN  .EQ.  -6  )  ILOW  =  0 

IF  (  ILEN  .EQ.  -5  .OR.  ILEN  .EQ.  -4  )  ILOW  =  -1 

IF  (  ILEN  .EQ.  -3  .OR.  ILEN  .EQ.  -2  )  ILOW  =  -2 

IF  (  ILEN  .EQ.  -1  .OR.  ILEN  .EQ.  0  )  ILOW  =  -3 

IF  (  ILEN  .EQ.  1  .OR.  ILEN  .EQ.  2  )  ILOW  =  -4 

IF  (  ILEN  .EQ.  3  .OR.  ILEN  .EQ.  4  )  ILOW  =  -5 

IriIGH  =  IABS(ILOW) 

DO  6  IWID  =  ILOW,  IHIGH 
NSUB  =  NSUB  +  1 

AZSUB(NSUB)  =  CAZ (1)  +  (FLOAT (ILEN) *DELTL) /MAGRTR 
ELSUB (NSUB)  =CEL(1)  +  (FLOAT (IWID) *DELTW) /MAGRTR 
RJSUB (NSUB)  =  RJNEW 
6  CONTINUE 

5  CONTINUE 

ENDIF 
ENDIF 
ELSE 

NOBJ  =  1 
ENDIF 

TARGET  GRAVITY  MODEL 
DO  10  I OBJ  =  1,  NOBJ 

MRTIC  =  DSQRT (RTIC ( 108 J, 1 ) *  *2  +  RTIC (IOBJ, 2) **2  + 

RTIC (IOBJ, 3) *’2) 

URTIC(l)  =  RTIC  (IOBJ,  1) /MRTIC 
URTIC (2)  =  RTIC (IOBJ, 2) /MRTIC 
URTIC ( 3 )  *  RTIC (IOBJ, 3) /MRTIC 

MGRT  -  GMU/MRTIC**2 
GRT (IOBJ, 1 )  =  -MGRT* URTIC ( 1 ) 

GRT ( IOBJ, 2 )  ■  -MGRT* URTIC ( 2 ) 

GRT (IOBJ, 3)  =  -MGRT'URTIC (3) 

INTEGRATE  TARGET  ACCELERATION  AND  VELOCITY  USING  AVERAGE 
GRAVITY  OVER  INTERVAL 

TDELT  =  T  -  TL1 
DO  2  I  »  1,3 

GRTAVG (IOBJ,  I)  =  0  5D0 * (GRT ( IOBJ,  I )  +  GRTLST ( IOBJ , I ) ) 

RTIC (IOBJ, I)  =  RTIC (IOBJ, I)  +  VTIC ( IOBJ, I ) *TDELT  + 

.  0 . 5 DO* GRTAVG ( IOBJ, I ) *TDELT*TDELT 

VTIC (IOBJ, I t  «  VTIC ( IOBJ, I )  +  GRTAVG (IOBJ, I) *TDELT 

2  CONTINUE 

C  SAVE  GRAVITY  VECTOR  FOR  USE  ON  NEXT  PASS 

DO  3  I  =  1,3 

GRTLST  (IOBJ,  I)  =*  GRT  ( IOBJ,  I  > 

3  CONTINUE 
10  CONTINUE 

TL1  =  T 

C  TRANSFORM  INERTIAL  POSITION  AND  VELOCITY  TO  E/ _'TH  FRAME 

RTAR(l)  =  RTIC (1, 1) *CIE (1)  +  RTIC ( 1 , 7 ) *CIE ( 4 )  +  RTIC (1 , 3) *CIE ( 7) 

RTAR(l)  -  RTICU,  1)  *CIE(2)  +  RTIC  ( 1 , 2)  *CIE  (  5)  +  RTIC  ( 1 ,  3)  *CIE  <  8) 

RTAR(l)  =  RTIC  (1,  1)  *CIE  (3)  +  RTIC  (1 , 2)  *CIE  {  6)  +  RTIC  (1  ,  3)  *CIE  <  9) 

VTAR!1)  =  VTIC (1, 1) *CIE(1)  +  VTIC (1 , 2) *CIE (4)  +  VTIC ( 1 , 3 ) *CIE ( 7 ) 

VTAR ( 2 )  »  VTIC (1, 1) *CIE (2)  +  VTIC (1, 2) *CIE (5)  +  VTIC ( 1 , 3 ) *CIE ( 8) 

VTAR ( 3 )  =  VTIC (1, 1) *CIE (3)  +  VTIC ( 1 , 2) *CIE (6)  +  VTIC (1, 3) *CIE (9) 

C  TRANSFORM  BODY  RATES  TO  EULER  RATES 
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TPHID  =  PTARG  +  QTARG'DSIN (TPHI ) 'DTAN (TTHT)  + 

RTARG'DCOS  (TPHI )  'DTAN  (TTHT) 

TTHTD  =  QTARG*DCOS (TPHI)  -  RTARG'DSIN (TPHI) 

TPSID  =  QTARG'DSIN (TPHI) /DCOS (TTHT)  +  RTARG*DCOS (TPHI ) /DCOS (TTHT) 

C  INTEGRATE  EULER  RATES  TO  OBTAIN  TARGET  ATTITUDE 

TPHI  -  TPHI  +  TPHID'TDELT 
TTHT  =  TTHT  +  TTHTD’TDELT 
TPSI  =  TPSI  +  TPSID*TDELT 

C  COMPUTE  TARGET  BODY -TO- INERTIAL  TRANSFORMATION  MATRIX 

SNGLTPHI  =  SNGL(TPHI) 

SNGLTTHT  =  SNGL(TTHT) 

SNGLTPSI  =  SNGL (TPSI ) 

CALL  MMK (SNGLTPHI, 1, SNGLTTHT, 2 , SNGLTPSI , 3 , CIT) 

CTI (1)  =  CIT  (1) 

CTI (2)  =  CIT (4) 

CTI  (3)  =  CIT (7) 

CTI (4)  =  CIT (2) 

CTI (5)  =  CIT (5) 

CTI (6)  =  CIT  (8) 

CTI (7)  =  CIT (3) 

CTI  (8)  =  CIT (6) 

CTI (9)  =  CIT (9) 

C  TRANSFORM  TARGET  EARTH  FRAME  POSITION  TO  ROTATING  EARTH 

RTER(l)  =  RTAR(l) *CER(1)  +  RTAR (2) -CER (4)  +  RTAR ( 3 ) *CER ( 7 ) 

RTER (2)  =  RTAR(l) *CER(2)  +  RTAR (2) 'CER (5)  +  RTAR ( 3) *CER ( 8 ) 

RTER ( 3 )  =  RTAR (1) *CER (3)  +  RTAR ( 2) 'CER ( 6)  +  RTAR ( 3 ) 'CER ( 9 ) 

C  CALCULATE  LATITUDE  AND  LONGITUDE  OF  TARGET 

LATT  =  DATAN2  (RTER  (3)  ,  DSQRT  (RTER  (I)  "2  +  RTER ( 2 )  "  2 ) ) / DTR 
LONGT  =  DATAN2 (RTER (2 ) , RTER ( 1 ) ) /DTR 

RETURN 

END 


FILE :  uuv22 .19g/dutility/uu timer. for 


SUBROUTINE  INITIALI2E_TIM£R ( ) 
SINCLUDE ( • :PFP: INCLUDE/TARGET. FOR' ) 
SINCLUDE  ( '  '■/INCLUOE/UUTIMER.COM'  ) 

$  INCLUDE  (  '  ’'/INCLUDE/SSCON22  .  DAT  *  ) 

SINCLUDE ( ■ "/INCLUDE/SSCON23. DAT' ) 

INTEGER  BN,  TN 

DO  20  BN  =  1 ,  4 
DO  10  TN=1 , 500 

NUMBER_TIMER (BN, TN)  =  0 
NUMBER_TICKS (BN, TN)  =  0 . 0D0 
10  CONTINUE 

20  CONTINUE 

STAGE1  =  INT4 (  TSTG1  *  1000.0  ) 
STAGE2  =  INT 4 (  TSTG2  *  1000.0  ) 

CALL  RESET_TIMER<) 

END 

SUBROUTINE  START  TIMER!  TN  ) 

SINCLUDE ( ' : PFP : INCLUDE7TARGET . FOR ' ) 
SINCLUDE ( ' "/INCLUDE/UUTIMER. COM ' ) 

INTEGER  TN 

TIMER (TN)  =  READ_TIM£R ( ) 

END 

SUBROUTINE  STOP_TIMER<  TN  ) 

SINCLUDE ( ' :PFP: INCLUDE/TARGET. FOR' ) 
SINCLUDE  (  '  'VINCLUDE/UUTIMER.COM'  ) 

INTEGER  TN 

TIMER (TN)  =  TIMER (TN)  -  READ  TIMER!) 


NUMBER  TIMER (4, TN) 


NUMBER  TIMER ( 4 , TN)  +  1 
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NUMBER_TICKS (4, TN)  =  NUM8ER_TICKS (4,  TN)  +  DBLE (TIMER (TN) ) 

IF  (  NUMBERJTIMER (4, TN)  . LT .  STAGE1  )  THEN 
NUMBER_TIMER ( 1 , TN)  =  NUMBERJTIMER (1, TN)  +  1 
NUMBER_TICKS (1, TN)  =  NUMBER_TICKS ( 1 , TN)  +  DBLE (TIMER (TN) ) 
ELSEIF  (  NUMBERJTIMER ( 4 ,  TN)  . LT .  STAGE2  )  THEN 
NUMBER_TIMER (2, TN)  =  NUMBER_TIMER (2,  TN)  +  1 
NUMBER_TICKS (2, TN)  =  NUMBER_TICKS (2, TN)  +  DBLE (TIMER (TN) ) 
ELSE 

NUMBER_TIMER (3, TN)  =  NUMBER_TIMER (3, TN)  +  1 
NUMBER_TICKS (3, TN)  =  NUMBERJTICKS <3,  TN)  +  DBLE (TIMER (TN) ) 
ENDIF 
END 

SUBROUTINE  OUTPUT_TIMER ( ) 

$ INCLUDE ( ' : PFP : INCLUDE/TARGET . FOR ' ) 

5 INCLUDE ( ' * / INCLUDE /UU TIMER . COM 1 ) 

INTEGER  BN,  TN 
INTEGER* 4  AVERAGE 

DO  20  TN-1,500 

IF  (  NUMBER_TIMER ( 4 , TN)  .NE.  0  )  THEN 

CALL  OUTPUT_MESSAGE(%VAL(SIGNED_16BIT) , TN, %VAL(INT2 (1) ) ) 
CALL  OUTPUT_MESSAGE (%VAL (CHARACTER_08BIT) ,  ’TIMER  •  ) 

DO  10  BN=1 , 4 

IF  (  NUMBERJTIMER (BN, TN)  . NE .  0  )  THEN 
AVERAGE  -  INT4 (NUMBER_TICKS (BN, TN) / 

&  DBLE  (NUMBERJTIMER  (BN,  TN)  )  ) 

ELSE 

AVERAGE  =  0 
ENDIF 

CALL  OUTPUT_MESSAGE (%VAL (SIGNED_32BIT)  ,  AVERAGE, 
i  %VAL ( INT2 ( 1 ) ) ) 

10  CONTINUE 

CALL  OUTPUT_NL 
END  IF 
20  CONTINUE 
END 


FILE :  uuv22 . 19g/dutility/uuvcsth2 .for 


C- 

C- 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  VCSTH2 (T, FLTC, FLTCP , FLTCY, TOFFLT, TIMONV) 


SUBROUTINE  NAME  :  VCSTH2 

AUTHOR (S)  :  B.  HILL 

FUNCTION  :  RESOLVES  THE  VCS  THRUSTER  BURN  TIMES  INTO 

THEIR  APPROPRIATE  FORCES  AND  MOMENTS 

CALLED  FROM  :  FORTRAN  MAIN 

SUBROUTINES  CALLED  :  TABLE 

INPUTS  :  T, FLTC, TOFFLT, TIMONV 

OUTPUTS  :  FLTCP, FLTCY 

BOTH  :  NONE 


UPDATES  : 


D. 

SISSOM 

- 

CR 

» 

017 

B. 

HILL 

- 

CR 

• 

030 

D. 

SISSOM 

- 

CR 

# 

032 

B. 

HILL 

- 

CR 

# 

038 

T. 

THORNTON 

- 

CR 

» 

043 

B. 

HILL 

CR 

# 

051 

B 

HILL 

- 

CR 

* 

057 

D. 

SMITH 

- 

CR 

* 

059 

D. 

SISSOM 

- 

CR 

» 

069 

D. 

SMITH 

- 

CR 

* 

074 

D. 

SMITH 

- 

CR 

# 

076 

D. 

SMITH 

- 

CR 

» 

080 

B. 

HILL  / 

- 

CR 

i 

081 

R. 

D. 

RHYNE 

SMITH 

_ 

CR 

# 

082 
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c 

R. 

RHYNE 

-  CR 

* 

084 

c 

B. 

HILL 

-  CR 

* 

086 

c 

R. 

RHYNE 

-  CR 

# 

087 

c 

B. 

HILL 

-  CR 

* 

089 

c 

B. 

HILL 

-  CR 

» 

093 

c 

c 


IMPLICIT  DOUBLE  PRECISION  (A-H) 

IMPLICIT  DOUBLE  PRECISION  (O-Z) 


DOUBLE  PRECISION  FLTC  (4) 

REAL  TIMONV  ,  TOFFLT ( 4 ) 


DO  10  1  =  1,  4 

IF  (  (TOFFLT (I) -T) . LE . 0.0  )  FLTC(I)  =  0.0 
10  CONTINUE 


IF  (  FLTC(l) .EQ.0.0  .AND.  FLTC (3 ) . EQ. 0 . 0  .AND. 

(TIMONV) .LE.T  )  FLTCY  =  0.0 
IF  (  FLTC(2) .EQ.0.0  .AND.  FLTC ( 4 ) . EQ . 0 . 0  .AND. 
(TIMONV) .LE.T  )  FLTCP  =0.0 


END 


FILE :  uuv22 . 19g/include/uu timer. com 


COMMON  /TIMER_COMMON/  STAGE1,  STAGE2,  TIMER,  NUMBER_TIMER,  NUMBER_TICKS 
INTEGER*4  STAGE1 ,  STAGE2 ,  TIMER(SOO),  NUMBER_TIMER ( 4 , 500 ) 

REAL* 8  NUMBERJTICKS (4,  500) 


FILE :  uuv22. 1 9g/sutility/ make  file 


FORFLAGS  =  code  large  optimize (3)  storage ( integer*2 ) 


OBJECTS  =  \ 

UUACSTHA . OBJ  \ 
UUACSTHB . OBJ  \ 
UUAERO. OBJ  \ 
UUATMOS1 . OBJ  \ 
UUATMOS2 . OBJ  \ 
UUBAUTO.OBJ  \ 
UUBGUID . OBJ  \ 
UUBRTAVG . OBJ  \ 
UUBSTEER . OBJ  \ 
UUBTHRST . OBJ  \ 
UUBXI2FV.OBJ  \ 
UUCORVEL.OBJ  \ 
UUCW87 . OBJ  \ 
UUFRACS . OBJ  \ 
UUFRCTHR . OBJ  \ 
UUFVDOT . OBJ  \ 
UtlGYRO .  OBJ  \ 
UUINTEG . OBJ  \ 
UUINTEGI . OBJ  \ 
UUM3X3I .OBJ  \ 
UUMCGUID.OBJ  \ 
UUMISSLR . OBJ  \ 
UUMMK . OBJ  \ 
UUMMLXY . OBJ  \ 
UUNCU . OBJ  \ 
UUNORM . OBJ  \ 
UUOUTMES . OBJ  \ 
UURAN . OBJ  \ 
UURAN0 . OBJ  \ 
UURAN I T . OBJ  \ 
UURESP2R . OBJ  \ 
UUROTMX . OBJ  \ 
UUSEEKEn . OBJ  \ 
UUSSPLAG . OBJ  \ 
UUTABLE . OBJ  \ 
UUTLU2EI . OBJ  \ 
UUVCSTH1 . OBJ  \ 
UUTIMER . OBJ 
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LIBRARY  =  UTILITY. LIB 


S (LIBRARY) : S (OBJECTS) 


. for . ob j : 

ftn286.new  $<  S(forflags) 

bnd286  S'.obj  name($*)  object (S' . Ink)  noload  noprint 

rename  S'.lnk  over  S'.obj 

submit  : PFP : csd/lib (  $ (LIBRARY) ,  $*  ) 


clean: 

delete  *. obj, *. 1st, $ (LIBRARY) 


FILE :  uuv22 . 19g/sutility/uuacstha. for 


SUBROUTINE  ACSTHA (T, CG, ACSLEV, DTACSA, TATAB, TOSEED, 

ITHRES, FXACS, FYACS, F2ACS, MXACS, MYACS, MZACS, 
MDOTA, IACSON) 


C- 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  NAME  : 
AUTHOR (S)  : 

FUNCTION  : 

CALLED  FROM  : 
SUBROUTINES  CALLED 
INPUTS  : 

OUTPUTS  : 

BOTH  : 


ACSTHA 
B.  HILL 

RESOLVES  THE  ACS  THRUSTER  BURN  TIMES  INTO 
THE  APPROPRIATE  FORCES  AND  MOMENTS 

FORTRAN  MAIN 

none 

T, CG, ACSLEV, DTACSA, TATAB 

FXACS, FYACS, FZACS, MXACS, MYACS, MZACS, MDOTA, 
IACSON 

TOSEED, ITHRES 


c 

UPDATES  : 

D. 

SISSOM 

- 

CR 

# 

017 

c 

D. 

SISSOM 

- 

CR 

# 

032 

c 

B. 

HILL 

- 

CR 

# 

038 

c 

T. 

THORNTON 

- 

CR 

i 

043 

c 

B. 

HILL 

- 

CR 

# 

051 

c 

D. 

SMITH 

- 

CR 

# 

059 

c 

D. 

SISSOM 

- 

CR 

# 

069 

c 

D. 

SMITH 

- 

CR 

I 

074 

c 

D. 

SMITH 

- 

CR 

* 

076 

c 

D. 

SMITH 

- 

CR 

# 

080 

c 

B. 

HILL  / 

- 

CR 

# 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH 

- 

CR 

# 

082 

c 

R. 

RHYNE 

- 

CR 

* 

083 

c 

R. 

RHYNE 

- 

CR 

* 

084 

c 

B. 

HILL 

- 

CR 

# 

086 

c 

R. 

RHYNE 

- 

CR 

t 

087 

c 

B. 

HILL 

- 

CR 

# 

089 

c 

B. 

HILL 

- 

CR 

ft 

093 

IMPLICIT  REAL 
IMPLICIT  REAL 

REAL  ACSDIR (3, 
REAL  AOFF1 ( 4 ) 
REAL  ATHRB ( 4 ! 
REAL 

REAL  ISPACS 
REAL  MXACS 
REAL  THACSA ( 8 , 
REAL  TMACSB ( 8 , 

INTEGER 
INTEGER 
INTEGER*  4 


(A-H) 

(O-Z) 

,  ACSLOC (3,4) 
,  AOFF214) 

,  CG ( 3 ) 

F  (3) 

,  M  ( 3 ) 

,  MYACS 
,  THACSB (8,4) 
,  XMOM ( 3 ) 

INDXA ( 4 ) 

LENA (4) 

TOSEED 


,  ACSMA  (9,4) 

,  ATHRA ( 4 ) 

,  DTACSA (4) 

,  FO ( 3 ) 

,  MDOTA 
,  MZACS 
,  TMACSA (8,4) 


INDXB ( 4 ) 
LENB ( 4 ) 


4) 


4) 

4) 
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C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IACSTH  ,  ACSMA 

*  DATA  INITIALIZATION 

SINCLUDE < ' "/INCLUDE/SSACSTHR . DAT ’ ) 

SINCLUDE ( 1 "/ INCLUDE /SSCONOl . DAT ' ) 

SINCLUDE  ( 1  "/INCLUDE/SSCON02  .  DAT  '  ) 

SINCLUDE  ( '"/INCLUDE/SSCON03.DAT1  ) 

SINCLUDE ( • "/INCLUDE/SSCON17.DAT'  ) 

SINCLUDE ( ’ "/INCLUDE/ SSCON18.DAT1 ) 

SINCLUDE ( ■ "/INCLUDE/SSCON1 9 . DAT  1 ) 

SINCLUDE ( ’ "/INCLUDE/SSCON20 . DAT  1 ) 

DATA  IACSTH  111 

IF  (  IACSTH. EQ.l  )  THEN 

IACSTH  =  0 

IF  (T  .LT.  TKVON+EPSL)  THEN 

C  ACS  MISALIGNMENT  DIRECTIONS 

C  AOFF1  =  CONE  ANGLE  OFF  NORMAL 

C  AOFF2  =  POLAR  ANGLE 

CALL  NORM (AOFFSD, 0.0, TOSEED, AOFF1 (1) ) 

CALL  NORM (AOFFSD, 0.0, TOSEED, AOFF1 (2) ) 

CALL  NORM (AOFFSD, 0.0, TOSEED, AOFF1 (3) ) 

CALL  NORM (AOFFSD , 0.0, TOSEED , AOFF1 ( 4 )  ) 

AOFF2 ( 1 )  =  2. 0*PI*RAN0 (TOSEED) 

AOFF2 (2)  =  2. 0*PI*RANO (TOSEED) 

AOFF2 ( 3 )  =  2. 0*PI*RAN0 (TOSEED) 

AOFF2 ( 4 )  =  2. 0*PI*RAN0 (TOSEED) 

ENDIF 

DO  300  1=1,4 

CAOFF1  »  COS (AOFF1 (I) ) 

SAOFF1  =  SIN (AOFF1  (I) ) 

CAOFF2  =  COS (AOFF2 (I) ) 

SAOFF2  =  SIN (AOFF2 (I) ) 

ACSMA  (1,1)  =  CAOFF1 
ACSMA (2,1)  =  SAOFF1 *CAOFF2 
ACSMA (3,1)  -  SAOFFl*SAOFF2 
ACSMA (4, I)  =  SAOFFl*SAOFF2 
ACSMA (5, I)  =  CAOFF1 
ACSMA (6, I)  =  SAOFF1 *CAOFF2 
ACSMA (7, I)  =  SAOFF1 *CAOFF2 
ACSMA (8, I)  =  SAOFFl*SAOFF2 
ACSMA (9, I)  =  CAOFF1 
300  CONTINUE 

ENDIF 

C  RESET  THE  FORCE  AND  MOMENT  COUNTERS  TO  ZERO 

FXACS  =  0.0 
FYACS  =0.0 
FZACS  =0.0 
MXACS  =0.0 
MYACS  =0.0 
MZACS  =  0.0 
MDOTA  =0.0 

IF  (ITHRES  .EQ.  1)  THEN 

*  The  ITHRES  assignment  was  moved  to  the  partition  with  MCAUTO,  KVAUTO 

*  ITHRES  =  0 

C  CALCULATE  TIME  FOR  PULSE  TO  COME  ON  AND  TIME  FOR  PULSE  TO 

C  REACH  FULL  FORCE  LEVEL 

TIMONA  =  TATAB  +  TLAGA 
TUPA  =  TIMONA  +  TRUPA 

C  DETERMINE  APPROPRIATE  MAXIMUM  THRUST  LEVEL 


IF  (ACSLEV  .GT.  1.5)  THEN 
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ACSF  =  ACSFH 
ELSE 

ACSF  =  ACSFL 
ENDIF 

DO  101  1=1,  4 

INITIALIZE  TABLE  POINTERS 

INDXAII)  =  1 
INDXB(I)  =  1 

CALCULATE  THRUSTER  RESPONSE  TABLE  FOR  "A"  THRUSTERS 

CALL  TABLE (TMACSA ( 1 , I) , THACSAtl, I) , TATAB, THAI, LENA (I) , 

INDXA (I) ) 

IF  (DTACSA  ( I )  .CE.  TCJ5INA)  THEN 
IF  (THAI  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  LOW 

TMACSA (1,1)  =  TATAB 
THACSA ( 1 , I)  =0.0 
TMACSA (2, I)  =  TIMONA 
THACSA (2, I)  =0.0 
TMACSA (3, I)  =  TUPA 
THACSA (3, I)  =  ACSF 
IPTR  =  4 
ELSE 

CAT  L  TABLE (TMACSA (1, I) , THACSA (1, I) .TIMONA, THA2 , 

LENA  (I)  ,  INDXA  (I)  ) 

IF  (THA2  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  EITHER  DELAY  OR  RAMP, 

AND  NO  CROSS-OVER  IS  PRESENT 

TMACSA (1,1)  =  TMACSA (LENA (I) -3, I) 

THACSA (1, 1)  =  THACSA(LENAU) -3, 1) 

TMACSA (2, I)  =  TMACSA < LENA (I) -7, T) 

THACSA (2,1)  =  THACSA (LENA (I) -2, I) 

TMACSA (3,1)  =  TMACSA (LENA (I) -1,1) 

THACSA (3,1)  =  THACSA (LENA(I) -1, I) 

TMACSA (4, I)  =  TIMONA 
THACSA (4,1)  =  0.0 
TMACSA (5, I)  =  TUPA 
THACSA (5, I)  =  ACSF 
IPTR  =  6 
ELSE 

CALL  TABLE (TMACSA (1, I) , THACSA (1, I) ,TUPA, THA3 , 

LENA (I), INDXA (I) ) 

IF  (THA3  .GE.  (ACSF-EPSL) )  THEN 

PREVIOUS  VALVE  STATE  WAS  HIGH 

TMACSA (1,1)  =  TATAB 
THACSA (1, I)  »  ACSF 
IPTR  =  2 
ELSE 

PREVIOUS  VALVE  STATE  WAS  DELAY,  AND  A 
CROSS-OVER  CONDITION  HAS  OCCURED 

TMACSA (1,1)  =  TMACSA (LENA (I) -3, I) 

THACSA (1,1)  =  THACSA(LENA(I)-3,I) 

TMACSA (2,1)  =  TMACSA (LENA (I) -2, I) 

THACSA  (2,  I)  =  THACSA  (LENA  (I) -2,  I) 

TMACSA (3,1)  =  (TMACSA (LENA (I) -1, I)  +  TIMONA1/2.0 
THACSA (3, I)  =  (TMACSA (3, I)  -  TIMONA) +ACSF/TRDNA 
TMACSA (4, I)  =  TUPA 
THACSA  (4, I)  =  ACSF 
IPTR  =  5 
ENDIF 
ENDIF 
ENDIF 

TMACSA (IPTR, I)  =  TIMONA  +  DTACSA ( I ) 

THACSA (IPTR, I )  =  ACSF 

TMACSA ( IPTR+ 1 , I )  =  TMACSA ( IPTR. I )  +  TRDNA 

THACSA (IPTR+ 1 , I)  =0.0 

TMACSA (IPTR+2, I)  =  999.0 

THACSA (IPTR+2, I)  =  0.0 

LENA ( I )  =  IPTR+2 


nn  non  n  nn  n  nn  nn 
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MAKE  SURE  VALVE  IS  OFF 

IF  (THAI  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  LOW 

TMACSA (1, I)  =  TATAB 
THACSA (1 , I)  =  0.0 
TMACSA (2,1)  =  999.0 
THACSA (2, I)  =0.0 
LENA  (I)  =  2 
ELSE 

CALL  TABLE (TMACSA (1 , I ) , THACSA (1 , I) , TUPA, THA3, LENA (I) , 
INDXA (I)  ) 

IF  (THA3  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  EITHER  DELAY  OR  RAMP,  WITH 
NO  CROSSOVER  POSSIBLE 


TMACSA (1, I) 
THACSA  (1,1) 
TMACSA (2, I) 
THACSA (2, I) 
TMACSA (3,  I) 
THACSA (3, I) 
TMACSA  (4,1) 
THACSA (4, I) 
LENA ( I )  =4 
ELSE 


TMACSA (LENA (I! -3, I) 
THACSA (LENA (I) -3, I) 
TMACSA (LENA (I) -2, I) 
THACSA (LENA (I) -2, I) 
TMACSA (LENA (I) -1, I) 
THACSA (LENA (I) -1, I) 
999.0 
0.0 


PREVIOUS  VALVE  STATE  WAS  DELAY,  AND  CROSSOVER  COULD 
OCCUR 

TMACSA (1,1)  =  TATAB 
THACSA (1, I)  =  ACSF 
TMACSA (2,1)  =  TIMuNA 
THACSA (2, I)  =  ACSF 
TMACSA (3,1)  =  TIMONA  +  TRDNA 
THACSA (3, I)  =  0.0 
TMACSA (4, I)  =  999.0 
THACSA (4, I)  =0.0 
LENA (I)  =  4 
ENDIF 
ENDIF 
ENDIF 


101  CONTINUE 


SET  REFERENCE  TIME  FOR  TABLE  LOOKUPS  AND  RESET  ACS  "ON"  FLAG 

TREF  =  T 
IACSON  =  0 

CALCULATE  AVERAGE  THRUST  LEVELS  FOR  EACH  "A"  THRUSTER 
DU.- TNG  NEXT  CYCLE 

DO  20  I  =  1  ,  4 

INITIALIZE  TABLE  POINTER 

INDXA(I)  =  1 

COMPUTE  INSTANTANEOUS  THRUST  LEVEL  VIA  TABLE  LOOKUP  IF  ACS  "A" 
CYCLE  IS  SCHEDULED  FOR  THIS  THRUSTER  .  ALSO  EXTRAPOLATE  TIME  OF 
NEXT  ACS  "A"  TABLE  LOOKUP  INDEX  TRANSITION  . 


IF  (  TMACSA (1, I) .GT. 0.0  )  THEN 

CALL  TABLE (TMACSA (1, I) , THACSA (1, I) , TREF, ATHRA ( I ) , 
LENA (I) , INDXA  (I) ) 

IF  (  ATHRA (I)  .GE.  ACSF-EPSL  )  IACSON  =  1 
ELSE 

ATHRA  (I)  =0.0 
INDXA ( I )  =  0 
ENDIF 


CALCULATE  THE  FORCES  AND  MOMENTS  PRODUCED  BY  THE  "A 
ACS  THRUSTERS  : 


o  o  o  o 
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F ( I )  IS  THE  FORCE  ALONG  THE  Ith  AXIS. 

XMOM(I)  IS  THE  EFFECTIVE  MOMENT  ARM. 

FORCES  ARE  ADJUSTED  FOR  MISALIGNMENT  EFFECTS. 

THE  MOMENT  GENERATED  IS  (  F  X  XMOM  ) . 

DO  10  J=1 , 3 

F0(J)  =  ACSDIR (J, I) *ATHRA (I) 

XMOM ( J)  =  CG(J)  -  ACSLOC ( J,  I ) 

10  CONTINUE 

F ( 1 )  =  ACSMAtl, I) *F0 (1)  +ACSMA (4, I) *F0 (2)  +ACSMA ( 7 , I ) *F0 ( 3 ) 

F (2)  =  ACSMA !2 , I ) *F0 (1 )  +ACSMA ( 5, I > *F0 ( 2 )  +ACSMA ( 8 , I ) *F0 ( 3 ) 

F ( 3 )  =  ACSMA (3, I) *F0  (1)  +ACSMA ( 6, I ) *F0 < 2)  + ACSMA ( 9, I ) *F0 ( 3 ) 

M(l)  =  F  (2 )  *XMOM  {  3 )  -  F(3)*XMOM<2) 

M  ( 2 )  =  F  ( 3 )  *XMOM  1 1  )  -  F(l)»XMOM(3) 

M { 3 )  =  F ( 1 ) *XMOM ( 2 )  -  F(2)*XMOM(l) 

FXACS  =  FXACS  +  F ( 1 ) 

FYACS  =  FYACS  +  F ( 2 ) 

F2ACS  -  FZACS  +  F(3) 

MX ACS  =  MXACS  +  M ( 1 ) 

MY ACS  -  MYACS  +  M (2 ) 

M2ACS  «  MZACS  +  M  ( 3 ) 

MDOTA  »  MDOTA  +  ATHRA { I ) /ISPACS 

20  CONTINUE 

RETURN 
END 


FILE:  uuv22 . 1 9g/sutility/uuacsthb. for 


SUBROUTINE 

ACSTHB (T, CG, ACSLEV, DTACSB, TATAB, TOSEED, 

ITHRES, FXACS, FYACS, F2ACS, MXACS, MYACS, MZACS, 

MDOTA, IACSON) 

c 

c 

SUBROUTINE 

NAME  :  ACSTHB 

c 

AUTHOR (S)  : 

B. 

HILL 

c 

FUNCTION  : 

RESOLVES  THE  ACS 

THRUSTER  BURN  TIMES  INTO 

c 

THE  APPROPRIATE  FORCES  AND  MOMENTS 

c 

CALLED  FROM 

:  FORTRAN  MAIN 

c 

SUBROUTINES 

CALLED  :  none 

c 

INPUTS  : 

T, 

CG, ACSLEV, DTACSB, 

,  TATAB 

c 

OUTPUTS  : 

FXACS, FYACS, FZACS, MXACS, MYACS, MZACS, MDOTA, 

c 

IACSON 

c 

BOTH  : 

TOSEED, ITHRES 

c 

UPDATES  : 

D. 

3ISSOM 

CR 

# 

017 

c 

D. 

SISSOM 

CR 

# 

032 

c 

B. 

HILL 

CR 

* 

038 

c 

T. 

THORNTON  - 

CR 

# 

043 

c 

B. 

HILL 

CR 

* 

051 

c 

D. 

SMITH 

CR 

* 

059 

c 

D. 

SISSOM 

CR 

* 

069 

c 

D. 

SMITH 

CR 

» 

074 

c 

D. 

SMITH 

CR 

# 

076 

c 

D. 

SMITH 

CR 

* 

080 

c 

B. 

HILL  / 

CR 

# 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH 

CR 

# 

082 

c 

R. 

RHYNE 

CR 

* 

083 

c 

R. 

RHYNE 

CR 

# 

084 

c 

B. 

HILL 

CR 

# 

086 

c 

R. 

RHYNE 

CR 

* 

087 

c 

B. 

HILL 

CR 

i 

089 

c 

c 

c--- 

B. 

IIILT  — 

CR 

» 

093 

IMPLICIT  REAL 


(A-H) 
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IMPLICIT  REAL  (O-Z) 

REAL  ACSDIR  (3,4)  ,  ACSLOC(3,4)  ,  ACSMA<9,4) 

REAL  AOFF1 (4)  ,  AOFF2(4)  ,  ATHRA ( 4 ) 

REAL  ATHRB ( 4 )  ,  CG<3) 

REAL  DTACSB  ( 4 )  ,  F(3)  ,  F0(3) 

REAL  ISP ACS  ,  M(3)  ,  MDOTA 

REAL  MX ACS  ,  MYACS  ,  MZACS 

REAL  THACSA(8,4)  ,  THACSB<8,4)  ,  TMACSA(8,4) 

REAL  TMACSB (8,4)  ,  XMOM(3) 

INTEGER  INDXA ( 4 )  ,  INDXB<4) 

INTEGER  LENA (4)  ,  LENB  < 4 ) 

INTEGER*  4  TOSEED 

C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IACSTH  ,  ACSMA 

*  DATA  INITIALIZATION 

SINCLUDE ( *  "/INCLUDE/ SSACSTHR.DAT ' ) 

SINCLUDE ( ’ * /INCLUDE/SSCONOl . DAT  1 ) 

SINCLUDE ( ' A/ INCLUDE/S SCON 02 . DAT  1 ) 

SINCLUDE ( 1 "/INCLUDE/SSCON03.DAT' ) 

SINCLUDE  (  1 "/INCLUDE/SSCON17.DAT1  ) 

SINCLUDE ( • "/INCLUDE/ SSCON18.DAT' ) 

SINCLUDE ( 1 "/INCLUDE/ SSCON19.DAT' ) 

SINCLUDE ( ' "/INCLUDE/SSCON20 . DAT • ) 

DATA  IACSTH  /  1  / 

IF  (  IACSTH. EQ.l  )  THEN 

IACSTH  =  0 

IF  (T  .LT.  TKVON+EPSL)  THEN 

C  ACS  MISALIGNMENT  DIRECTIONS 

C  AOFF1  =  CONE  ANGLE  OFF  NORMAL 

C  AOFF2  -  POLAR  ANGLE 

CALL  NORM (AOFFSD, 0.0, TOSEED, AOFF1 (1) ) 

CALL  NORM (AOFFSD, 0.0, TOSEED, AOFF1 (2) ) 

CALL  NORMIAOFFSD, 0.0, TOSEED, AOFF1 (3) ) 

CALL  NORM (AOFFSD, 0 . 0 , TOSEED, AOFF1 (4) ) 

AOFF2 ( 1 )  =  2. 0*PI*RAN0 (TOSEED) 

AOFF2 ( 2 )  =  2. 0*PI*RAN0 (TOSEED) 

AOFF2  (3)  =  2. 0*PI*RAN0  (TOSEED) 

AOFF2 ( 4 )  =  2. O*PI*RAN0 (TOSEED) 

ENDIF 

DO  300  1=1,4 

CAOFF1  =  COS (AOFF1 (I) ) 

SAOFF1  =  SIN (AOFF1 (I) ) 

CAOFF2  =  COS (AOFF2 (I) ) 

SAOFF2  =  SIN (AOFF2 (I) ) 

ACSMA (1, I)  =  CAOFF1 
ACSMA (2,1)  =  SAOFF1 *CAOFF2 
ACSMA (3, I)  =  SAOFF1 *SAOFF2 
ACSMA (4,1)  =  SAOFFl*SAOFF2 
ACSMA (5, I)  =  CAOFF1 
ACSMA (6, I)  =  SAOFF1 *CAOFF2 
ACSMA (7, I)  -  SAOFF1 *CAOFF2 
ACSMA (8, I)  =  SAOFFl*SAOFF2 
ACSMA (9, I)  =  CAOFF1 
300  CONTINUE 

ENDIF 

C  RESET  THE  FORCE  AND  MOMENT  COUNTERS  TO  ZERO 


FXACS 

a* 

0.0 

FYACS 

a 

0.0 

FZACS 

* 

0.0 

MXACS 

= 

0.0 

MYACS 

a 

0.0 

MZACS 

= 

0.0 

MDOTA 

0.0 

oo  o  non  non 
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IF  (ITHRES  .EQ.  1)  THEN 

*  The  ITHRES  assignment  was  moved  to  the  partition  with  MCAUTO,  KVA'JTO 
ITHRES  =  0 

CALCULATE  TIME  FOR  PULSE  TO  COME  ON  AhD  TIME  FOR  PULSE  TO 
REACH  FULL  FORCE  LEVEL 

T I MONA  =  TATAB  +  TLAGA 
TUPA  =  TIMONA  +  TRUPA 

DETERMINE  APPROPRIATE  MAXIMUM  THRUST  LEVEL 

IF  (ACSLEV  .GT.  1.5)  THEN 
ACSF  =  ACSFH 
ELSE 

ACSF  =  ACSFL 
ENDIF 

DO  101  1=1,4 

INITIALIZE  TABLE  POINTERS 

INDXA(I)  =  1 
INDXB(I)  =  1 

CALCULATE  THRUSTER  RESPONSE  TABLE  FOR  "B"  THRUSTERS 

CALL  TABLE  (TMACSB  (1,1),  THACSB  (1,1),  TATAB,  THB1 ,  LENB  ( I)  , 
INDXB (I) ) 

IF  (DTACSB(I)  .GE.  ICMINA)  THEN 
IF  (THB1  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  LOW 

TMACSB (1,1)  =  TATAB 
THACSB  (1,1)  =  0.0 
TMACSB (2, I)  =  TIMONA 
THACSB (2, I)  =0.0 
TMACSB (3, I)  =  TUPA 
THACSB (3, I)  =  ACSF 
IPTR  =  4 
ELSE 

CALL  TABLE (TMACSB (i, I) , THACSB (1, I) , TIMONA, THB2 , 

LENB ( I ) , INDXB ( I ) ) 

IF  (THB2  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  EITHER  DELAY  OR  RAMP, 

AND  NO  CROSS-OVER  IS 

TMACSB (1,1)  =  TMACSB (LENB(I) -3, I) 

THACSB (1,1)  =  THACSB (LENB (I) -3, I) 

TMACSB (2,1)  =  TMACSB (LENB (I) -2, I) 

THACSB (2,1)  =  THACSB (LENB (I ) -2, I) 

TMACSB (3,1)  =  TMACSB (LENB(I) -1,  I) 

THACSB (3,1)  =  THACSB (LENB (I) -1, I) 

TMACSB (4, I)  »  TIMONA 
THACSB (4, I)  =0.0 
TMACSB (5, I)  =  TUPA 
THACSB (5, I)  =  ACSF 
IPTR  =  6 
ELSE 

CALL  TABLE (TMACSB (1, I) , THACSB ( 1 , I) , TUPA, THB3 , 

LENB ( I ) , INDXB  ( I ) ) 

IF  (THB3  .GE.  (ACSF-EPSL) )  THEN 

C  PREVIOUS  VALVE  STATE  WAS  HIGH 

TMACSB (1,1)  =  TATAB 
THACSB (1, I)  =  ACSF 
IPTR  =  2 
ELSE 

C  PREVIOUS  VALVE  STATE  WAS  DELAY,  AND  A 

C  CROSS-OVER  CONDITION  HAS  OCCURED 

TMACSB (1,1)  =  TMACSB (LENB (I ) -3, I) 

THACSB (1,1)  =  THACSB (LENB (I ) -3, I) 

TMACSB (2,1)  =  TMACSB (LENB (I ) -2, I ) 

THACSB (2,1)  =  THACSB (LENB (I) -2, I) 

TMACSB (3,1)  =  (TMACSB (LENB (I) -1, I)  +  TIMONA) 


non  no  no 


19.  Appendix  N:  EXOS1M  2.0  (End-to-end) 


129 


n.z 

THACS3  (3,1)  =  ( TMACS3  (3,1)  -  TIMONA) 'ACSF/TRDNA 
TMACSB (4, It  =  T'JPA 
THACS3 ( 3 , I)  =  ACSF 
IPTR  =  5 
ENDIF 
ENDIF 
ENDIF 

TMACSB (IPTR, I)  =  TIMCNA  +  DTACSB(I) 

THACSB (IPTR, I )  =  ACSF 

TMACSB (IPTR+1, I)  =  TMACSB ( IPTR, I )  -  TRDNA 
THACSB (IPTR+1, I)  =0.0 
TMACSB (IPTR+2, I)  =  999. C 
THACSB (IPTR+2, I)  =  0.0 
LENB(I)  =  IPTR+2 
ELSE 

C  MAKE  SURE  VALVE  IS  OFF 

IF  (THB1  . LT.  EPSL)  THEN 

C  PREVIOUS  VALVE  STATE  WAS  LOW 

TMACSB (1,1)  =  TATAB 
THACSB (1,1)  =  0.0 
TMACSB (2, I)  =  999.0 
THACSB <2, I >  =0.0 
LENB(I)  =  2 
ELSE 

CALL  TABLE (TMACSB (1, I) , THACSB (1,1), TUPA, TH31, LENB (I) , 
INDXB(D) 

IF  (THB3  .LT.  EPSL)  THEN 

PREVIOUS  VALVE  STATE  WAS  EITHER  DELAY  OR  RAMP,  WITH 
NO  CROSSOVER  POSSIBLE 

TMACSB (1,1)  =  TMACS3 (LENB (I ) -3, I) 

THACSB (1,1)  =  THACSB (LENB (X) -3, I) 

TMACSB (2,1)  =  TMACSB (LENB (I) -2, I) 

THACSB ( 2,1)  =  THACSB (LENB (I) -2, I) 

TMACSB (3, I)  =  TMACSB (LENB (I) -1, I) 

THACSB (3,1)  «  THACSB (LENB (1) -1, I) 

TMACSB (4,1)  =  999.0 
THACSB (4, I)  =0.0 
LENB (I)  =  4 
ELSE 

PREVIOUS  VALVE  STATE  WAS  DELAY,  AND  CROSSOVER  COULD 
OCCUR 

TMACSB (1,1)  =  TATAB 
THACSB (1, I)  =  ACSF 
TMACSB (2, I )  =  TIMCNA 
THACSB (2, I)  =  ACSF 
TMACSB (3,1)  =  TIMCNA  +  TRDNA 
THACSB (3, I)  =  0.0 
TMACSB (4, I )  =  999.0 
THACSB (4, I)  =  0.0 
LENB (I)  =  4 
ENDIF 
ENDIF 
ENDIF 

101  CONTINUE 
ENDIF 

SET  REFERENCE  TIME  FOR  TABLE  LOOKUPS  AND  RESET  ACS  "ON"  FLAG 

TREF  =  T 
IACSON  =  0 

CALCULATE  AVERAGE  THRUST  LEVELS  FOR  EACH  "B”  THRUSTER 
DURING  NEXT  CYCLE 

DO  40  I  =  1  ,  4 

INITIALIZE  TABLE  POINTERS 

INDXB(I)  =  1 


C 


COMPUTE  INSTANTANEOUS  THRUST  LEVEL  VIA  TABLE  LOOKUP  IF  ACS  "B" 
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C  YC  Lt.  IS  SCHl Dij-C-O  r  .>R  .  ri  -  ^  .  r.  R ..  o  .  r  :*  •  A ..b 0  r, X  .  A r' G . . A  .  ■,  I.Xi 
NEXT  ACS  "3"  TABLE  LOCKS?  INCEX  TRANSITION  . 

IF  (  TMACSB(l.I) .GT.C.3  )  THEN 

CALL  TABLE (TMACSB (1, I) , THACS3 ( 1 , I ) . TREE, ATHRB 1 1/  , 

LENB(I)  ,  INDXB(I)  ) 

IF  (  ATHRB(I)  .GE.  ACSF-EPSL  )  IACSON  =  I 
ELSE 

ATHRB (I)  =0.0 
INDXB(I)  =  0 
ENDIF 

CALCULATE  THE  FORCES  AND  MOMENTS  PRODUCED  3Y  THE  "3" 

ACS  THRUSTERS  : 

F ( I )  IS  THE  FORCE  ALONG  THE  Ich  AXIS. 

XMOM(I)  IS  THE  EFFECTIVE  MOMENT  ARM. 

FORCES  ARE  ADJUSTED  FOR  MISALIGNMENT  EFFECTS. 

THE  MOMENT  GENERATED  IS  (  F  x  XMCM  ) . 

DO  30  J=I,3 

F  0 ( J )  =  -ACSDIR ! J, I ) *ATHR3 ( I ) 

XMOM(J)  =  CG  ( J)  -  ACSLOCIJ,!) 

30  CONTINUE 


F  (1) 

=  ACSMA  ( 1 ,  I)  *F0(1 

)  *ACSMA (4, I) 

*FC  (2)  ‘ACSMA  (J,I) 

* F0  (3 

F  (2) 

=  ACSMA  (2,  II  *F0  (I 

)  ‘ACSMA (5, I) 

*F0(2>  ‘ACSMA  (8,1) 

*FC  ' 3 ; 

F  (3) 

=  ACSMA (3, I) -F0 (I 

)  ‘ACSMA (6,1) 

"F0 (2)  ‘ACSMA (9, I ) 

*FC  (3; 

M  ( 1 ) 

=  F ( 2 ) *XMOM ( 3 )  - 

F (3! * XMCM (2) 

M  (2) 

=  F ( 3 ) *XMCM(1)  - 

F (1 ) ’XMCM (3) 

M  ( 3 ) 

=  F(l)  *XMOM(2)  - 

F  (2)  ’XMCM  (I } 

FXACS  =  FXACS  *  F(l) 

FYACS  =  FYACS  +  F (2) 

FZACS  =  FZACS  -  F ( 3 ) 

MXACS  =  MXACS  *  Mil) 

MYACS  =  MYACS  ‘  M<2) 

MZACS  =  MZACS  *  M { 3 ) 

MDOTA  =  MDOTA  *  ATHR3 ( I ) / 1 3? ACS 

40  CONTINUE 

RETURN 

END 


C 

C 


C 

C 


noannnnnon 
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D. 

SMITH 

-  CR 

07  6 

D. 

SMITH 

-  CR 

# 

080 

B. 

R. 

HILL  / 
RHYNE 

-  CR 

* 

081 

R. 

RHYNE 

-  CR 

» 

087 

B. 

HILL 

-  CR 

* 

089 

B. 

HILL 

-  CR 

# 

093 

B. 

HILL 

-  CR 

# 

095 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 


REAL 

CA1M (205) 

,  CA2M (205) 

,  CATAB (205) 

REAL 

CG  (3) 

,  CNAK205) 

,  CNA2 (205) 

REAL 

CF  TAB (205) 

,  CPTAB (205) 

,  MACH 

REAL 

MACHL 

,  MVRWM 

,  MXA 

REAL 

MYA 

,  MZA 

REAL 

VRWM ( 3 ) 

,  XCPL1  (205) 

,  XCPL2  (205) 

LOCAL 

DATA  USED  FOR 

CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE 

CATAB 

,  CNTAB 

,  CPTAB 

ICAM 

,  ICAA 

,  ICNM 

ICNA 

,  ICPM 

,  ICPA 

*  DATA  INITIALIZATION 
SINCLUDE ( 1 " / INCLUDE/SSAERO . DAT ' ) 
$INCLUDE ( ' VINCLUDE/SSC0N21 . DAT • ) 
$INCLUDE ( ■ */ INCLUDE/ SSCON22 . DAT  * ) 
$INCLUDE  ( '  'VINCLUDE/SSCON23  .  DAT  *  ) 
SINCLUDE ( *  A / INCLUDE /SSCON2 4 . DAT  1 ) 
SINCLUDE  ( 1  '‘/INCLUDE/SSCON25  .  DAT  1  ) 
SINCLUDE ( *  A/ INCLUDE/S SCON 71 .DAT  * ) 

IF  (IAERO  .EQ.  1)  THEN 
I AERO  =  0 

IF  (T  .LT.  TSTG1)  THEN 

SUR  =  SREF1 
DO  10  1=1,205 

CATAB(I)  =  CAIM(I) 
CNTAB (I)  =  CNA1 (I) 

CPTAB(I)  =  XCPL1  (I) 
10  CONTINUE 


ELSE 


20 


SUR  =  SREF2 
DO  20  1=1,205 
CATAB(I) 
CNTAB (I)  = 

CPTAB(I) 
CONTINUE 


CA2M ( I ) 
CNA2 (I) 
XCPL2  (I) 


ENDIF 


ICAM  =  1 

ICAA  =  1 

ICNM  =  1 

ICNA  =  1 

ICPM  =  1 

ICPA  =  1 

ENDIF 

CALCULATE  DYNAMIC  PRESSURE  AND  MACH  NUMBER 


QA  =  (MVRWM**2) *RHO/2.0 

MACH  =  MVRWM/VSND 

C  ZERO  AERO  FORCES  AND  MOMENTS  WHEN  MISSILE  VELOCITY  IS  ZERO 


IF  (  MVRWM.LE.0.0  .OR.  ( ABS ( T-TSTG2) . LE . DTEPS)  )  THEN 


FXA 

=  0.0 

FYA 

=  0.0 

FZA 

=  0.0 

MXA 

=  0.0 

MYA 

=  0.0 
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MZA  =  0.0 
ELSE 

C  COMPUTE  TOTAL,  PITCH,  AND  YAW  ANGLES  OF  ATTACK 

TMP1  =  SQRT  (  VRWM(2)**2  +  VRWM(3)**2  ) 

ALFAT  =  ATAN2  (  TMP1  ,  ABS (VRWM ( 1 ) )  )  /  DTR 

ALFAP  =  ATAN2  (  VRWM (3)  ,  VRWM(l)  )  /  DTR 

ALFAY  =  ATAN2  (  VRWM (2)  ,  SQRT  (  VRWM (1) '*2 

+  VRWM(3) **2)  )  /  DTR 

IF  (  ABS(TMPl) .GT.1.0E-6  )  THEN 
CPHIA  =  VRWM (3)  /  TMP1 
SPHIA  =  VRWM (2)  /  TMP1 
ELSE 

CPHIA  =1.0 
SPHIA  =  0.0 
ENDIF 

C  AXIAL  FORCE  COEFFICIENT  -  F (M, A) 

CALL  TLU2EI  (  MACH,  ALFAT,  CATAB,  ICAM,  ICAA,  CA  ) 

C  NORMAL  FORCE  COEFFICIENT  -  F (M, A) 

CALL  TLU2EI  (  MACH,  ALFAT,  CNTAB,  ICNM,  ICNA,  CN  ) 

C  CENTER-OF-PRESSURE  FOR  PITCH  AND  YAW  FORCE  -  F (M, A) 

CALL  TLU2EI  (  MACH,  ALFAT,  CPTAB,  ICPM,  ICPA,  XCP  ) 

C  COMPUTE  AERODYNAMIC  FORCES 


QS 

=  QA*SUR 

FXA 

=  QS*CA 

FYA 

=  -QS*CN*SPHIA 

FZA 

=  -QS'CN'CPHIA 

COMPUTE 

AERODYNAMIC  MOMENTS 

MXA 

=  FYA*CG<3)  - 

FZA*CG<2) 

MYA 

=  -FXA*CG<3)  + 

FZA* (  CG ( 1 ) 

-  XCP 

MZA 

=  FXA*CG<2)  - 

FYA*  (  CG(1) 

-  XCP 

ENDIF 

RETURN 

END 


FILE:  uuv22 . 1 9g/sut i 1 i ty /uuatmosl .for 


C - 

SUBROUTINE  ATMOS1 (T, ALT, RHO, PRESS, VSND) 

C - 

c 

C  SUBROUTINE  NAME  :  ATMOS1 

C 

C  AUTHOR (S)  :  DAVID  C.  FOREMAN 

C 

C  FUNCTION  :  COMPUTES  ATMOSPHERIC  PROPERTIES  AS  A 

C  FUNCTION  OF  ALTITUDE 

C 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  TABLE 

C 

C  INPUTS  :  T, ALT 

C 

C  OUTPUTS  :  RHO, PRESS, VSND 

C 


c 

UPDATES  : 

T. 

THORNTON 

-  OR 

# 

003 

c 

T. 

THORNTON 

-  CR 

# 

016 

c 

D. 

SMITH 

-  CR 

# 

027 

c 

B. 

HILL 

-  CR 

# 

030 

c 

B. 

HILL 

-  CR 

H 

036 

c 

T. 

THORNTON 

-  CR 

# 

037 

c 

T. 

THORNTON 

-  CR 

# 

042 

c 

D. 

SMITH 

-  CR 

* 

059 

c 

D. 

SISSOM 

-  CR 

# 

069 

c 

D. 

SMITH 

-  CR 

# 

076 

ooooooooo 
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D.  SMITH  -  CR  *  080 
B.  HILL  /  -  CR  *  081 
R.  RHYNE 

R.  RHYNE  -  CR  #  087 
B.  HILL  -  CR  #  089 
B.  HILL  -  CR  »  093 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  ALTT  (59)  ,  CIM(9) 

REAL  CRW  ( 9)  ,  CWR(9) 

REAL  LONG 

REAL  RHOT (59)  ,  SHEART (59) 

REAL 

REAL  VWINDT (59) 

*  DATA  INITIALIZATION 
SINCLUDE ( 1 A/ INCLUDE /SSATMOS . DAT  1 ) 

5INCLUDE ( 1 */INCLUDE/SSCON21 . DAT  1 ) 

SINCLUDE ( 1 ^ /INCLUDE /SSCON2  6 . DAT ' ) 

$INCLUDE ( ' VINCLUDE/SSCON27 . DAT ' ) 

DATA  IALT/  1  / 

C  DETERMINE  ATMOSPHERIC  DENSITY 

CALL  TABLE (ALTT, RHOT, ALT, RHO, 59,  IALT) 

C  DETERMINE  ATMOSPHERIC  PRESSURE 

CALL  TABLE (ALTT, PRESST, ALT, PRESS, 59, IALT) 

C  DETERMINE  SPEED  OF  SOUND 

CALL  TABLE (ALTT, VSNDT, ALT, VSND, 59, IALT) 

RETURN 

END 


FILE :  uuv22 . 19g/sut ility /uuatmos2 .for 


,  CRI (9) 

,  lAT 

PRESST  (59) 
,  UVRWM ( 3 ) 
VSNDT  (59) 
WINDRT (59) 


C - 

SUBROUTINE  ATMOS2 (T, ALT, XD, YD, ZD, CIM, CRI, LAT, LONG, 

VWIND, SHEAR, VRWM, MVRWM) 

c - 

C 

C  SUBROUTINE  NAME  :  ATMOS 

C 

C  AUTHOR (S)  :  DAVID  C.  FOREMAN 

C 

C  FUNCTION  :  COMPUTES  ATMOSPHERIC  PROPERTIES  AS  A 

C  FUNCTION  OF  ALTITUDE 

C 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  TABLE  ,  MMK 

C 

C  INPUTS  :  T, ALT, XD, YD, ZD, CIM, CRI, LAT, LONG 

C 

C  OUTPUTS  :  VWIND, SHEAR, VRWM, MVRWM 

C 

C  UPDATES  :  T.  THORNTON  -  CR  i  003 

C  T.  THORNTON  -  CR  #  016 


c 

D. 

SMITH 

- 

CR 

* 

027 

c 

B. 

HILL 

- 

CR 

* 

030 

c 

B. 

HILL 

- 

CR 

# 

036 

c 

T. 

THORNTON 

- 

CR 

t 

037 

c 

T. 

THORNTON 

- 

CR 

* 

042 

c 

D. 

SMITH 

- 

CR 

* 

059 

c 

D. 

SISSOM 

- 

CR 

# 

069 

c 

D. 

SMITH 

- 

CR 

# 

076 

c 

D. 

SMITH 

- 

CR 

i 

080 

c 

B. 

HILL  / 

- 

CR 

# 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

- 

CR 

* 

087 

c 

B. 

HILL 

- 

CR 

# 

089 
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C  B.  HILL  -  CR  #  093 

C 

c - 

c 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  ALTT  (59)  ,  CIM(9)  ,  CRI<9) 

REAL  CRW  (9)  ,  CWR(9)  ,  LAT 

REAL  LONG  ,  MVRWM  ,  PRESST (59) 

REAL  RHOT  (59)  ,  SHEART  (5  9)  ,  UVRWM(3> 

REAL  VIWIND ( 3 )  ,  VRWI ( 3 ) 

REAL  VRWIND  ( 3 )  ,  VRWM(3)  ,  VSNDT  (59) 

REAL  VWINDT  (59)  ,  VWWIND(3)  ,  WINDRT(59) 

*  DATA  INITIALIZATION 

$ INCLUDE ( * A /INCLUDE /SSATMOS . DAT ' ) 

SINCLUDE ( ' VINCLUDE/SSCON21 . DAT • ) 

$INCLUDE ( ■ A/INCLUDE/SSCON26. DAT ' ) 

SINCLUDE ( 1 A/INCLUDE/SSCON27.DAT' ) 

DATA  IALT/  1  / 

C  DETERMINE  LOCAL  WIND  VELOCITY 

CALL  TABLE (ALTT, VWINDT, ALT , VWIND, 59, IALT) 

C  DETERMINE  HORIZONTAL  WIND  DIRECTION 

CALL  TABLE (ALTT, WINDRT, ALT, WINDIR, 5 9,  IALT) 

SWDIR  »  SIN (WINDIR*DTR) 

CWDIR  =  COS (WINDIR‘DTR) 

C  DETERMINE  VERTICAL  WIND  COMPONENT 

CALL  TABLE (ALTT, SHEART, ALT, SHEAR, 59, IALT) 

C  COMPUTE  THE  TRANSFORMATION  FROM  THE  WIND  FRAME  TO  INERTIAL  AND 

C  VICE  VERSA 

CALL  MMK (0.0, 1, -LAT*DTR, 2, LONG*DTR, 3, CRW) 

CWR(l)  a  CRW  (1 ) 

CWR ( 2 )  -  CRW (4) 

CWR ( 3 )  -  CRW ( 7 ) 

CWR ( 4 )  a  CRW (2) 

CWR ( 5 )  «  CRW ( 5 ) 

CWR ( 6)  =  CRW ( 8 ) 

CWR  (7)  =  CRW  (3) 

CWR (8)  =  CRW (6) 

CWR (9)  -  CRW (9) 

C  CALCULATE  THE  WIND  VELOCITY  IN  WIND  FRAME 

VWWIND ( 1 )  =  SHEAR 
VWWIND (2)  =  CWDIR*VWIND 
VWWIND (3)  =  SWDIR*VWIND 

C  COMPUTE  WIND  VELOCITY  IN  THE  INERTIAL  FRAME 

VRWIND (1)  =  CWR (1) ‘VWWIND (1)  +  CWR ( 4 ) ‘VWWIND (2 )  +  CWR ( 7 ) ‘VWWIND ( 3 ) 

VRWIND (2)  =  CWR(2) ‘VWWIND (1)  +  CWR ( 5) ‘VWWIND (2 )  +  CWR ( 8 ) ‘VWWIND ( 3 ) 

VRWIND ( 3 )  =  CWR (3) ‘VWWIND (1)  +  CWR ( 6) ‘VWWIND (2 )  +  CWR ( 9) ‘VWWIND ( 3 ) 

VIWIND (I)  =  CRI (1) ‘VRWIND (I)  +  CRI ( 4 ) * VRWIND (2)  +  CRI (7) ‘VRWIND (3) 

VIWIND ( 2 )  =  CRI (2) ‘VRWIND (1)  +  CRI (5 ) ‘VRWIND ( 2 )  +  CRI ( 8 ) ‘VRWIND ( 3 ) 

VIWIND (3)  =  CRI (3) ‘VRWIND (1)  +  CRI ( 6) ‘VRWIND ( 2)  +  CRI ( 9) ‘VRWIND ( 3 ) 

C  COMPUTE  WIND  RELATIVE  MISSILE  VELOCITY 

VRWI (i)  =  XD  -  VIWIND (1) 

VRWI (2)  =  YD  -  VIWIND (2) 

VRWI (3)  =  ZD  -  VIWIND (3) 

VRWM(l)  =  CIM (1 ) ‘VRWI ( 1 )  +  CIM (4) ‘VRWI (2)  +  CIM ( 7 ) ‘VRWI ( 3 ) 

VRWM ( 2 )  =  CIM(2) ‘VRWI (1)  +  CIM ( 5 ) ‘VRWI ( 2 )  +  CIM ( 8 ) ‘ VRWI ( 3 ) 

VRWM ( 3 )  =  CIMI3) ‘VRWI (1)  +  CIM ( 6 ) * VRWI ( 2 )  +  CIM ( 9 ) * VRWI ( 3 ) 

MVRWM  =  SQRT  (  VRWM  (1)  “2  +  VRWM  (2)  “2  ♦  VRWM  ( 3 )  “  2  ) 

IF  (  MVRWM. GT. 0.0  )  THEN 

*  FTN286  X41 5  OPTIMIZEO) 


UUUUUUUUUUUUUUC'JUUUUUUUUUUUUUUUUUUUUUUU 
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99999  CONTINUE 

UVRWM ( 1 ) 

=  VRWM(l) 

/ 

MVRWM 

UVRWM ( 2  > 

=  VRWM ( 2 ) 

/ 

MVRWM 

UVRWM (3) 

=  VRWM (3) 

/ 

MVRWM 

ELSE 

UVRWM ( 1 ) 

=  0.0 

UVRWM ( 2 ) 

=  0.0 

UVRWM ( 3 ) 

=  0.0 

ENDIF 


RETURN 

END 


FILE :  uuv22 . 19g/sutility/uubauto. for 


c - 

SUBROUTINE  BAUTO <T, THTER, PSIER, SQ, SR, MASS, IYY, IZZ, CGEST, TI2M, RMIR, 
VMIR, IBAUTO, CMMD, DLPC, DLYC, KTHT, KTHTD, XDEL, 

XCPCG, LFRACS, CNALP, MDELTA, KNE, KME, MALPHA) 


SUBROUTINE  NAME  : 
AUTHOR (S)  : 
FUNCTION  : 

CALLED  FROM  : 
SUBROUTINES  CALLED 


BAUTO 

L.  C.  HECK,  D.  C.  FOREMAN 

PROVIDES  CONTROL  OF  THE  MISSILE  ABOUT  THREE 
AXES  THROUGHOUT  THE  BOOST  PHASE  OF  FLIGHT 

FORTRAN  MAIN 

TABLE  ,  TLU2EI  ,  OPTSCC 


INPUTS 


T, THTER, PSIER, SQ,  SR, MASS, IYY, IZZ, CGEST, TI2M, 
RMIR, VMIR, IBAUTO 


OUTPUTS 


CMMD, DLPC, DLYC, KTHT, KTHTD, XDEL, XCPCG, 
LFRACS, CNALP, MDELTA, KNE, KME, MALPHA 


UPDATES  : 


T. 

THORNTON 

- 

CR 

# 

025 

D. 

SMITH 

- 

CR 

# 

027 

T. 

THORNTON 

- 

CR 

# 

037 

B. 

HILL 

- 

CR 

# 

038 

D. 

SMITH 

- 

CR 

# 

039 

T. 

THORNTON 

- 

CR 

* 

042 

T. 

THORNTON 

- 

CR 

# 

046 

T. 

THORNTON 

- 

CR 

# 

048 

B. 

HILL 

- 

CR 

* 

056 

D. 

SMITH 

- 

CR 

* 

059 

D. 

SI3S0M 

- 

CR 

* 

069 

D. 

SMITH 

- 

CR 

# 

072 

B. 

HILL  / 

- 

CR 

« 

081 

R. 

RHYNE 

R. 

RHYNE 

- 

CR 

* 

087 

B. 

HILL 

- 

CR 

* 

089 

D. 

SMITH 

- 

CR 

# 

092 

B. 

HILL 

- 

CR 

# 

093 

IMPLICIT  REAL 

(A-H) 

IMPLICIT  REAL 

(O-Z) 

REAL 

CGEST  (3) 

,  CMMD (2) 

TI2M  ( 9) 

REAL 

KTHT 

,  KPSI 

KTHTD 

REAL 

KPSID 

,  TIMTE1 (26) 

r 

TIMTE2  (29) 

REAL 

THRTE1 (26) 

,  THRTE2  (29) 

t 

ALTT  (59) 

REAL 

CNA1E (205) 

,  CNA2E (205 ) 

t 

XCPL1E  (205) 

REAL 

XCPL2E  (205) 

,  RHOT (59) 

! 

VMIR (3) 

REAL 

LD 

,  KNE 

t 

KME 

REAL 

LFRACS 

,  FRCLOC  (3,4) 

MALPHA 

REAL 

MDELTA 

,  VSNDT (59) 

r 

PRESST (59) 

REAL 

MCHLIM 

,  CA1ME (205) , 

CA2ME (205) 

REAL 

IYY 

,  IZZ 

REAL 

AFR (3, 3) 

,  BFR  (3,1) 

§ 

KFRfl,  3) 

REAL 

Q1FR (3, 3) 

,  Q2FR (1,1) 

MASS 

REAL 

RMIR (3) 

,  VMRWE ( 3 ) 

REAL 

CNTABE (205) 

,  CPTABE (205) 

, 

CATABE (205) 

C 
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SAVE  ICNME,  ICNAE,  ICPME,  ICPAE, 

ICAME,  ICAAE,  IALTE,  ITH1E,  ITH2E 

*  DATA  INITIALIZATION 

SINCLUDE  ( '  "'/INCLUDE/SSCON33 .  DAT  '  ) 

SINCLUDE  ( ’  ''/INCLUDE/ S SCON 3 4  .  DAT  1  ) 

SINCLUDE ( 1 VINCLUDE/SSCON35.DAT' ) 

SINCLUDE ( ■  VINCLUDE/SSCON21 . DAT ' ) 

SINCLUDE  ( '  'VINCLUDE/SSCON22  .DAT  1  ) 

SINCLUDE  ( •  A/INCLUDE/SSCON24.DAT'  ) 

SINCLUDE  ( •  'VINCLUDE/SSCON26 .  DAT  1  ) 

SINCLUDE ( 1 " /INCLUDE/SSCON28 . DAT  1 ) 

SINCLUDE ( 1  * /INCLUDE /SSCON2 9 . DAT  1 ) 

SINCLUDE ( • */ INCLUDE /SSCON3 0 . DAT 1 ) 

SINCLUDE ( 1 “ /INCLUDE /SSCON31 . DAT1 ) 

SINCLUDE ( 1 " /INCLUDE /SSCON32 . DAT  1 ) 

SINCLUDE ( ■ VINCLUDE/SSOPTSSC.FUN1 ) 

IF  (IBAUTO  .EQ.  1)  THEN 

IBAUTO  =  0 

IF  (T  .LT.  TSTG1 )  THEN 
DO  10  1=1,205 

CNTABE ( I )  =  CNAIE(I) 

CPTABE(I)  »  XCPL1E ( I ) 

CATABE (I )  =  CAIME(I) 

10  CONTINUE 

ELSE 

DO  20  1=1,205 

CNTABE (I)  =  CNA2E ( I ) 

CPTABE(I)  =  XCPL2E  (I) 

CATABE (I)  =  CA2MEII) 

20  CONTINUE 

ENDIF 

ICNME  =  1 

ICNAE  =  1 

ICPME  =  1 

ICPAE  =  1 

ICAME  =  1 

ICAAE  =  1 

IALTE  =  1 

ITH1E  •=  1 

ITH2E  =  1 

ENDIF 

C  COMPUTE  ESTIMATED  ALTITUDE 

ESTALT  =  SQRT  (  RMIR(1)**2  +  RMIR(2)**2  +  RMIR(3)**2  )  -  RADE 

C  COMPUTE  ESTIMATED  ATMOSPHERIC  PROPERTIES 

CALL  TABLE (ALTT,RHOT  , ESTALT, ESTRHO, 59, IALTE) 

CALL  TABLE ( ALTT, PRESST, ESTALT, ESTPRE, 59, IALTE) 

CALL  TABLE (ALTT, VSNDT  , ESTALT, ESTVSD, 59, IALTE) 

C  COMPUTE  ESTIMATED  WIND  RELATIVE  VELOCITY  COMPONENTS 

VMRWE(l)  =  VMIR(l) *TI2M(1)  +  VMIR (2) *TI2M (4)  +  VMIR (3) *TI2M (7) 

VMRWE ( 2 )  =  VMIR(l) *TI2M(2)  +  VMIR (2) *TI2M (5)  +  VMIR ( 3) *TI2M ( 8 ) 

VMRWE ( 3 )  =  VMIR(l) *TI2M(3)  +  VMIR < 2 ) *TI2M < 6)  +  VMIR ( 3 ) *TI2M ( 9) 

C  COMPUTE  ESTIMATED  MACH  NUMBER  AND  DYNAMIC  PRESSURE 

ESTVEL  =  SQRT  (  VMRWE (1)**2  +  VMRWE<2)**2  +  VMRWE(3)**2  ) 

ESTMCH  =  ESTVEL/ESTVSD 

ESTQA  =  £STRHO*ESTVEL*  *2/2 . 0 

C  CALCULATE  ESTIMATED  VACUUM  THRUST 

IF  (  T.GE.TSTG1  )  THEN 
TO  =  T  -  TST20N 
SREFE  =  SREF2 
XNOZE  =  XNOZ2 
AEXITE  =  AEXIT2 

CALL  TABLE (TIMTE2 , THRTE2 , TO , THRVE, 29, ITH2E) 

ELSE 

TO  =  T  -  TIGN 

SREFE  =  SREF1 


o  o 
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XN'OZE  =  XNOZ1 
AEXITE  =  AEXIT1 

CALL  TABLE (TIMTE1 , THRTE1 , TO , THRVE, 2 6, ITH1E) 

ENDIF 

CONI  L'TE  ECTI.iATLL  T'  LIVEREC  ■.  r.-  jsi 

THRE  =  T.iRVE  -  AEXITE*ESTPRE 
IF  (  THRE.  LT.  0.0  )  THRE  =  0.0 

COMPUTE  ESTIMATED  TOTAL  ANGLE  OF  ATTACK  IN  DEGREES  AND  PLANAR 
ANGLES  OF  ATTACK  IN  RADIANS  . 

IF  (  ESTVEL.GT.0.0  )  THEN 

ALFATE  =  ATAN2  (  SQRT (VMRWE (2) **2  +  VMRWE (3) ’*2) , 

ABS (VMRWE ( 1 ) )  ) /DTR 

ALFAPE  =  ATAN2  (  VMRWE (3)  ,  VMRWE (1)  ) 

ALFAYE  =  ATAN2  (  -  VMRWE (2)  ,  VMRWE (1)  ) 

ELSE 

ALFATE  =0.0 
ALFAPE  =0.0 
ALFAYE  =0.0 
ENDIF 

CALL  TLU2EI  (  ESTMCH,  4.0  ,  CNTABE  ,  ICNME,  ICNAE,  CNE  ) 

CALL  TLU2EI  (  ESTMCH,  ALFATE,  CP T ABE  ,  ICPME,  ICPAE,  XCPE  ) 

CALL  TLU2EI  (  ESTMCH,  ALFATE,  CATABE  ,  ICAME,  ICAAE,  CAE  ) 

C  CALCULATE  CNAtFA  (PER  RADIAN) 

CNALP  =  CNE/ (4.0*DTR) 

C  ESTIMATE  DRAG  FORCE 

DRAGE  =  CAE*ESTQA*SREFE 

C  COMPUTE  AERODYNAMIC  MOMENT  ARM 

XCPCG  =  XCPE  -  CGEST(l) 

C  TVC  AUTOPILOT 

IF  (  T.LT.TFRCS  -AND.  T.GE.TINHIB  )  THEN 
XDEL  =  CGEST(l)  -  XNOZE 

MALPHA  =  ABS <CNALP*XCPCG*SREFE’ESTQA/IYY) 

KTHT  =  -  <IYY*WMTVC**2  +  CNALP *SREFE*ESTQA*XCPCG) 

/ (THRE*XDEL) 

KPSI  =  -  (IZZ*WMTVC**2  *  CNALP *SREFE*ESTQA*XCPCG) 

/ (THRE*XDEL) 

KTHTD  =  -  2.0*ZETTVC*WMTVC*IYY/ (THRE*XDEL) 

KPSID  =  -  2 . 0 *ZETTVC*WMTVC* IZZ / (THRE*XDEL) 

C  AUTOPILOT  PITCH  AND  YAW  CONTROL  FOR  THRUST  VECTOR  CONTROL 

CMMD(l)  =  THTER*KTHT  -  SQ*KTHTD 
CMMD ( 2 )  =  PSIER*KPSI  -  SR'KPSID 

C  COMPUTE  BUCKET  LIMIT  ON  NOZZLE  COMMANDS 

TOTCMD  =  SQRT ( CMMD ( 1 ) *  *  2  +  CMMD(2)**2) 

IF  (  TOTCMD. GT.BCKLMT  )  THEN 

CMMD (1 )  =  CMMD (1) *BCKLMT/TOTCMD 
CMMD (2)  =  CMMD (2) *BCKLMT/TOTCMD 
ENDIF 

ELSE 

KTHT  =  4.0 

KPSI  =4.0 

KTHTD  =4.0 
KPSID  =  4.0 
CMMD ( 1 )  =  0.0 
CMMD (2)  =0.0 

ENDIF 

C  FORWARD  REACTION  CONTROL  SYSTEM  AUTOPILOT 


IF  (  T.GE.TFRCS  )  THEN 
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COMPUTE  FORCE  AND  MOMENT  MULTIPLIERS 

LD  =  (  XJET  -  XNOZE  ) /DJET 

CT  =  THJET/ (ESTQA*SJET) 

TMP1  =  SQRT  (  CT  ) 

IF  (  ESTMCH . LE . MCHLIM  )  THEN 

KNE  -  C.C118  +  (0.1358*<i.  3 . 485*SQET  (LD)  )  /  T”!-- / 

+  0.0946*£STMCH  +  0.004317/LD 

ELSE 

KNE  =  1.0  +  EXP (1.1  -  0.2116* (ALOG (CT) +8.5) **1. 4) 
ENDIF 

KME  =  0.5582  -  0.1884/TMP1  -  1.9659/LD 

DETERMINE  AIRFRAME  COEFFICIENTS  FOR  PLANT  MODEL 
NOTE  :  AN  ALTERNATE  CALCULATION  FOR  MDELTA  IS 
MDELTA  =  (-KME'DJET  +  KNE*LFRACS) /IYY 

TMP1  =  ESTQA*SREFE*CNALP 

TMP2  =  MASS'ESTVEL 

LFRACS  -  FRCLOC (1,1)  -  CGEST(l) 

MALPHA  «  TMP1*XCPCG/IYY 

MDELTA  =  -  KNE*LFRACS/IYY 

ZALPHA  =  (  THRE  +  DRAGE  +  TMP1  ) /TMP2 

ZDELTA  «  -  KNE/TMP2 

ESTIMATE  MAXIMUM  ANGLE  OF  ATTACK 

ALFAMX  -  ABS  (  THJET* MDELTA /MALPHA  ) 

SET  PLANT  WEIGHTING  MATRIX 

Q1FR (1,1)  =  1 . 0/THERMX**2 
Q1FR (2,2)  =  1 . 0/THDTMX*  *2 
Q1FR (3,3)  *  1 . 0/ALFAMX**2 

SET  INPUT  WEIGHTING  MATRIX 

Q2FR (1,1)  =  1.0/(KNE*THJET)**2 

INITIALIZE  ANALOG  PLANT  MODEL 

AFR  (1.1)  «  0.0 

AFR  (1,2)  -  1.0 

AFR  (1,3)  -  0.0 

AFR  (2.1)  =  0.0 

AFR  (2.2)  =  0.0 

AFR (2, 3)  «  MALPHA 

AFR (3,1)  »  0.0 

AFRO, 2)  =  1.0 

AFRO,  3)  =  -  ZALPHA 

BFR (1,1)  =  0.0 

BFR (2,1)  =  MDELTA 

BFR (3,1)  =  -  ZDELTA 

COMPUTE  STEADY  STATE  OPTIMAL  CONTROL  GAINS 

CALL  OPTSSC (AFR, BFR, 3,1, DTAPU, Q1FR, Q2FR,  KFR) 

KFR (1,1)  =  OPTSSC1 (ESTALT) 

KFR (1,2)  -  OPTSSC2 (ESTALT) 

KFR (1,3)  -  OPTSSC3 (ESTALT) 

COMPUTE  DESIRED  PLANAR  CONTROL  FORCES 

FCMDP  =■  KFR  (1,1)  *THTER  -  KFR(1,2)*SQ  -  KFR  ( 1 ,  3 )  *  ALFAPE 
FCMDY  *  -  KFR (1, 1) *PSIER  +  KFR(1,2)*SR  +  KFR ( 1 , 3 ) * ALFAYE 

COMPUTE  DURATION  OF  NEEDED  VALVE  OPEN  PULSES 

DLPC  =  FCMDP/ (KNE*THJET) 

DLYC  =  FCMDY/ (KNE*THJET) 

ELSEIF  (  T.LT.TFRCS  )  THEN 
DLPC  >=0.0 
DLYC  >=  0.0 
ENDIF 

RETURN 

END 
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FILE :  uuv22 . 19g/sut ility/uubguid. for 


c - 

SUBROUTINE  BGUID <T( AT, AC, TI2M, PG, IMINSF, VW, PGD, VWD, WC. PSIER, 
TH»fck,PM,KA,i:V) 


c — 

c 

c 

SUBROUTINE  NAME  : 

BGUID 

c 

AUTHOR (S)  : 

L. 

C.  HECK,  D.  C. 

FOREMAN 

c 

FUNCTION  : 

TO 

CALCULATE  THE 

ERROR  BETWEEN  THE  COMMANDED 

c 

POINTING  VECTOR  AND  THE  ACTUAL  DIRECTION  THE 

c 

MISSILE  IS  POINTED 

DURING  BOOST 

c 

CALLED  FROM  : 

FORTRAN  MAIN 

c 

SUBROUTINES  CALLED  : 

TABLE,  intrinsics 

c 

INPUTS  : 

T, AT, AC, TI2M 

c 

OUTPUTS  : 

PGD, VWD,  WC, PSIER, 

THTER, PM, KA, KV 

c 

BOTH  ; 

PG, 

,  IMINSF, VW 

c 

UPDATES  : 

T. 

THORNTON  -  CR 

« 

006 

c 

T. 

THORNTON  -  CR 

# 

016 

c 

T. 

THORNTON  -  CR 

# 

025 

c 

B. 

HILL  -  CR 

# 

030 

c 

T. 

THORNTON  -  CR 

# 

037 

c 

T  . 

THORNTON  -  CR 

« 

042 

c 

T. 

THORNTON  -  CR 

# 

046 

c 

D. 

SMITH  -  CR 

# 

059 

c 

D. 

SMITH  -  CR 

* 

072 

c 

B. 

HILL  /  -  CR 

* 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH  -  CR 

# 

092 

c 

B. 

HILL  -  CR 

# 

093 

c 

c— 

— 

_ 

IMPLICIT  REAL 

(A-H) 

IMPLICIT  REAL 

(O-Z) 

REAL 

VWD (3) 

VW  ( 3 ) 

,  WC  ( 3) 

REAL 

PGD (3) 

PG  (3) 

,  PM  (3) 

REAL 

AT  { 3 ) 

AC  (3) 

,  TI2M i 9! 

REAL 

KA  , 

KA1 

,  KA2 

REAL 

KA3  , 

KA4 

,  KA5 

REAL 

KV 

KV1 

,  KV2 

REAL 

KV3 

KV4 

,  KV5 

REAL 

ATTLTT ( 5 ) 

ATTLMT  (5) 

*  DATA  INITIALIZATION 

SINCLUDE ( ' A 

/INCLUDE/SSCON36 

•  DAT'  ) 

SINCLUDE ( ■ * 

/INCLUDE/SSCCN37 

.DAT' ) 

SINCLUDE {•" 

/ INCLUDE /SSCON3 8 

. DAT ' ) 

SINCLUDE ( ' A 

/INCLUDE/SSCON22 

. DAT ' ) 

S INCLUDE (,A 

/INCLUDE/SSCON30 

•  DAT'  ) 

DATA 

IATTLM  /  1  / 

C  COMPUTE  POINTING  VECTOR  COMMAND  GAINS 

IF  (  T.LE.TS  )  THEN 
KA  =  KA1 

KV  =  KV1 

ELSEIF  (  T.LE.TSTG1  )  THEN 
KA  =  KA2 

KV  =  KV2 

ELSEIF  (  T.LE.T5  )  THEN 
KA  -  KA3 

KV  =  KV3 

ELSEIF  (  T.LE.T2S  )  THEN 
KA  «  KA4 

KV  =  KV4 

ELSE 

KA  =  KA5 

KV  =  KV5 

ENDIF 
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C  COMPUTE  COMMAND  ANGULAR  VELOCITY  INTEGRAL  (VW>  DERIVATIVE 

VWD(l)  =  AT  (2)  *AC  (3)  -AT(3)*AC(2) 

VWD<2)  -  AT<3)*AC(1)  -  AT(1)*AC(3) 

VWD  (3)  =  AT(1)*AC(2)  -AT(2)*AC(1) 

C  LAUNCH  STEERING  MODE 

IF  (  T.LE.TC  )  THEN 

C  COMPUTE  THE  COMMAND  ANGULAR  VELOCITY  VECTOR  (WC) 

WC<1)  =  0.0 

WC ( 2 )  =  0.0 
WC ( 3 )  =  0.0 

C  COMFUTE  POINTING  VECTOR  (PG)  DERIVATIVE 

PGD(l)  =  0.0 

PGD (2)  -  0.0 

PGD ( 3 )  ”0.0 

C  MINIMUM  IMPULSE  STEERING  MODE 

ELSEIF  (  T.LE.TS  )  THEN 

C  RESET  POINTING  ON  FIRST  PASS  THROUGH  MINS  LOGIC 

IF  (  IMINSF.EQ. 0  )  THEN 
PG ( 1 )  =  AC  ( 1 ) 

PG  (2)  -  AC  (2) 

PG  (3)  =  AC  (3) 

IMINSF  =  1 
ENDIF 

C  COMPUTE  THE  COMMAND  ANGULAR  VELOCITY  VECTOR  (WC) 

WC<1)  =  KA*VWD(1)  +  KV*VW(1) 

WC  (2)  =  KA*VWD<2)  +  KV*VW(2) 

WC ( 3 )  -  KA*VWD(3)  +  KV*VW(3) 

WCMAX  -  AMAX1  (ABS  (WC  (1 )  ) ,  ABS  (WC (2) ) ,  ABS  (WC  ( 3)  )  ) 

IF  (  WCMAX. GT.WLIM  )  THEN 
SCALE  -  WLIM/WCMAX 
WC(1)  -  SCALE*WC(1) 

WC  (2)  =  SCALE*WC(2) 

WC ( 3 )  =  SCALE*WC(2) 

ENDIF 

C  COMPUTE  POINTING  VECTOR  (PG)  DERIVATIVE 

PGD  ( 1 )  =  WC (2 ) *PG ( 3 )  -  WC (3) *PG (2) 

PGD  (2)  «WC(3)*PG(1)  -WC(1)*PG(3) 

PGD ( 3 )  =  WC ( 1 ) *PG (2 )  -  WC(2)*PG(1) 

C  SET  POINTING  VECTOR  COINCIDENT  WITH  STEERING  VECTOR  DURING 

C  FRACS 

IF  (  T.GE.TFRCS  )  THEN 
PG ( 1 )  «  AC ( 1 ) 

PG  (2)  »  AC  (2) 

PG  ( 3 )  =  AC  (3) 

ENDIF 

C  GENERALIZED  ENERGY  MANAGEMENT  STEERING  MODE 

ELSEIF  (  T.LE.TCD  )  THEN 

C  COMPUTE  COMMAND  ANGULAR  VELOCITY  INTEGRAL  (VW) 

VWMAX  »  AMAX1  (ABS  (VW  (1)  )  ,  ABS  (VW(2)  )  ,  ABS  (VW(3)  )  ) 

IF  (  VWMAX. GT.VWLIM  )  THEN 
SCALE  =  VWLIM/VWMAX 
VW(1)  =  SCALE*VW(1) 

VW  ( 2 )  =  SCALE*VW(2) 

VW  (  3 )  =  SCALE*VW(3) 

ENDIF 

C  COMPUTE  THE  COMMAND  ANGULAR  VELOCITY  VECTOR  (WC) 

WC(1)  -  KA*VWD(1)  +  KV*VW(1) 


L>  U 
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WC  ( 2 )  =  KA'VWD  (2  )  +  KV*VW(2) 

WC(3)  =  KA*VWD<3)  +  KV*VW(3) 

WCMAX  =  AMAX1 (ABS (WC (1) ) , ABS (WC (2) ) , ABS (WC (3) ) ) 
IF  (  WCMAX. GT.WLIM  )  THEN 
SCALE  =  WLIM/WCMAX 
WC(1)  -  C„\LE*WC(1) 

WC (2 )  =  SCALE*WC(2) 

WC ( 3 )  =  SCALE*WC(3) 

ENDIF 

C  COMPUTE  POINTING  VECTOR  (PG)  DERIVATIVE 

PGD(l)  =  WC  (2)  *PG  (3)  -  WC  (3)  *PG  (2) 

PGD(2)  =  WC  (3)  *PG  ( 1 )  -WC(1)*PG(3) 

PGD  ( 3 )  -  WC  ( 1 )  *PG  ( 2 )  -WC(2)*PG(1) 

C  COUNTDOWN  STEERING  MODE 


ELSE 

C  COMPUTE  COMMAND  ANGULAR  VELOCITY  INTEGRAL  (VW) 

VWMAX  -  AMAX1  (ABS  (VW(1)  )  ,  ABS  (VW  (2)  )  ,  ABS  (VW  (3)  )  ) 

IF  (  VWMAX. GT.VWLIM  )  THEN 
SCALE  -  VWLIM/ VWMAX 
VW(1)  =  SCALE*VW(1) 

VW  (2)  =  SCALE*VW(2) 

VW ( 3 )  =  SCALE*VW(3) 

ENDIF 

C  COMPUTE  THE  COMMAND  ANGULAR  VELOCITY  VECTOR  (WC) 

WC ( 1 )  =  G.O 

WC ( 2 )  =  0.0 

WC (3)  =  0.0 

C  COMPUTE  POINTING  VECTOR  (PG)  DERIVATIVE 

PGD (1 )  =  0.0 
PGD (2)  =0.0 
PGD (3)  =  0.0 
ENDIF 

TRANSFORM  THE  POINTING  VECTOR  FROM  THE  INERTIAL  GUIDANCE  FRAME 
INTO  THE  MISSILE  BODY  FRAME 

PM ( 1 )  =  PG(1) *TI2M(1)  +  PG ( 2 ) *TI 2M ( 4 )  +  PG (3) *TI2M (7) 

PM ( 2 )  =  PG ( 1 ) *TI2M ( 2 )  +  PG ( 2 ) *TI2M ( 5 )  +  PG (3) *TI2M (8) 

PM  (3)  =  PG(1)  *TI2M<3)  +  PG  ( 2 )  *TI2M  (  6)  +  PG  (3)  *TI2M  (  9) 

C  COMPUTE  THE  ERROR  SIGNAL  SENT  TO  THE  AUTOPILOT 

PSIER  =  PM (2) 

THTER  =  -PM (3) 

C  LIMIT  ATTITUDE  ERRORS  SENT  TO  THE  AUTOPILOT 

CALL  TABLE (ATTLTT, ATTLMT, T, ATTLM, 5, IATTLM) 

TOTERR  =  SQRT  (  PSIER**2  +  THTER* * 2  ) 

IF  (  TOTERR. GT. ATTLM  )  THEN 
PSIER  =  PSIER* ATTLM/ TOTERR 
THTER  =  THTER*ATTLM/TOTERR 
ENDIF 

RETURN 

END 


FILE:  uuv22.19g/suti 1 i ty /uubrtavg .for 


c - 

SUBROUTINE  BRTAVG (TN, TA, DT, W) 

c - 

c 

C  SUBROUTINE  NAME  :  BRTAVG 

C 

C  AUTHOR (S)  :  D.  F.  SMITH 

C 

C  FUNCTION  :  Compute  the  average  body  rates  over  the  last 

C  interval  using  the  current  and  previous 


no  no  no  o  o o o o o n o n n o o o n 
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inertial  to  missile  transformation  matrices 
CALLED  FROM  :  GYRO 

SUBROUTINES  CALLED  :  M3X3I 

INPUTS  :  TN, TA, DT 

OUTPUTS  :  W 

UPDATES  :  D.  SMITH  -  CR  #  076 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  TN  ( 9)  ,  TA(9),  W  <  3 ) 

REAL  TD  ( 9)  ,  TI(9),  TE(9) 

COMPUTE  INVERSE  OF  PREVIOUS  TRANSFORMATION  MATRIX 

CALL  M3X3I  (  TA  ,  TI  ) 

COMPUTE  DELTA  ROTATION  MATRIX  FROM  PREVIOUS  MISSILE  ATTITUDE  TO  CURRENT 
MISSILE  ATTITUDE 

TD  (1 )  -  TN  ( 1 )  -TI  ( 1 )  +  TN(4)*TI(2)  +  TN(7)*TI(3) 

TD  (2 )  -  TN(2)*TI(1)  +  TN(5)*TI(2)  +  TN(B)*TI(3) 

TD  (3)  =  TN(3)*TI(1)  +  TN(6)'TI(2)  +  TN(9)*TI(3> 

TD  <  4 )  =  TN<1)'TI<4)  +  TN  (4 )  *TI  ( 5 )  +  TN(7)*TI(6) 

TD  ( 5)  -  TN  (2)  *TI  (4)  +  TN(S)'TI(5)  +  TN(8)*TI(6) 

TD(6)  -  TN(3)*TI<4)  +  TN(6)*TI(5)  +  TN(9)*TI(6> 

TD  (7)  -  TN  (1)  'TI  (7)  +  TN(4)*TI<8)  +  TN  (7)  *TI  ( 9) 

TD  ( 8 )  =  TN(2)*TI(7)  +  TN(5)*TI(8)  +  TN  (8)  *TI  <  9) 

TD  ( 9)  -  TN(3)*TI(7)  +  TN(6)*TI(8)  +  TN(9)*TI(9) 

DETERMINE  DELTA  EULER  ANGLES  FROM  PREVIOUS  ORIENTATION  (  EULER  ROTATION 
SEQUENCE  IS  PSI-THETA-PHI  ) 

DLPSI  .  AT  AN  2  (  TD(4)  ,  TD  <  1 )  ) 

DLTHE  =  A?TN  (  -TD(7)  ) 

DLPHI  =  ATAN2  (  TD  ( 8 )  ,  TD(9)  ) 

CDLPSI  -  COS  (  DLPSI  ) 

SDLPSI  -  SIN  (  DLPSI  ) 

CDLTHE  =  COS  (  DLTHE  ) 

SDLTHE  -  SIN  (  DLTHE  ) 

CDLPHI  =  COS  (  DLPHI  ) 

SDLPHI  »  SIN  (  DLPHI  ) 

COMPUTE  MATRIX  RELATING  EULER  ANGULAR  RATES  TO  BODY  RATES  (  ( TE 1  IS 
USED  FOR  TEMPORARY  STORAGE  ) 

TE ( 1 )  =  1.0 

TE (2 )  =  0.0 

TE  ( 3 )  =  0.0 

TE ( 4 )  =  0.0 

TE ( 5 )  =  CDLPSI 

TE  ( 6)  =  -  SDLPSI 

TE ( 7 )  =  -  SDLTHE 

TE (8)  -  CDLTHE 'SDLPHI 

TE ( 9)  =  CDLTHE 'CDLPHI 

C  ADD  IDENTITY  MATRIX  TO  [TEJ  AND  INVERT  THE  RESULTANT  MA1RIX 

TD ( 1 )  =  TE ( 1 )  +  1.0 

TD  (2)  -  TE  ( 2) 

TD ( 3 )  »  TE<3) 

TD ( 4 )  «  TE { 4 ) 

TD(5)  =  TE(5)  +  1.0 

TD  (  6)  -  TE  ( 6 ) 

TD { 7 )  -  TE ( 7 ) 

TD  <  8 )  *  TE ( 8 ) 

TD  (  9)  -  TE  (  9)  +1.0 

CALL  M3X3I  (  TD  ,  TI  ) 

C  CALCULATE  AVERAGE  BODY  RATES  OVER  LAST  INTERVAL 

TD(1)  -  TKl)'TE(l)  +  TI  (  4 )  *  TE  ( 2  )  +  TI  (7)  *  TE  (3) 
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TD  (2) 

= 

TI (2) *TE  (1) 

♦ 

TI (5) *TE (2) 

+ 

TI (8) -TE (3) 

TD  (3) 

= 

TI (3) *TE  (1) 

+ 

TI (6) *TE  (2) 

+ 

TI (9) "TE  (3) 

TD  (4) 

TI (1) *TE  (  4 ) 

+ 

TI (4) *TE (5) 

+ 

TI  (7)  *TE  (6) 

TD  (5) 

= 

TI (2) *TE (4) 

+ 

TI (5) *TE (5) 

+ 

TI  (St  *TE  (6) 

TD  !£) 

= 

TI  (3)  *TE<4) 

+ 

TI (6) *TE (5) 

+ 

TI (9) *TE (6) 

TD  (7) 

= 

TI (1) *TE (7) 

+ 

TI (4) *TE (8) 

+ 

TI  (7)  *T£<9) 

TD  (8) 

* 

TI (2) *TE  (7) 

+ 

TI (5) *TE  (8) 

-f 

TI (8) *TE (9) 

TD  (9) 

= 

TI (3) *TE (7) 

+ 

TI  (6)  *TE<8) 

4- 

TI  (9)  *TE  (9) 

W<1) 

SB 

2.0  *  (  TD  (1) 

*DLPHI  v  TD ( 4  ) 

*  DLTHE  +  TD (7) 

*DL?SI  ) 

/ 

DT 

w  (2) 

SB 

2.0  *  (  TD ( 2 ) 

'DLPHI  +  TD (5) 

'DLTHE  +  TD  ( 8) 

*DLPSI  ) 

/ 

DT 

W  ( 3) 

= 

2.0  *  (  TD ( 3 ) 

•DLPHI  +  TD ( 6) 

'DLTHE  +  TD ( 9) 

*DLPSI  ) 

/ 

DT 

RETURN 

END 

FILE :  uuv22 . 1 9g/ sut i 1 ity /uubsteer . for 


SUBROUTINE  BSTEER (T, USI, USF, UVS, MVS, MVR, AT, RMIR, VMIR, US, US o, AC, 
WASTAN, VRATIO, VELWD) 


SUBROUTINE  NAME  : 
AUTHOR (S)  : 

FUNCTION  : 

CALLED  FROM  : 
SUBROUTINES  CALLED 
INPUTS  : 

OUTPUTS  : 

BOTH  : 


BSTEER 

L.  C.  HECK,  D.  C.  FOREMAN 

CALCULATES  THE  STEERING  COMMANDS  FOR  THE 
BOOST  PHASE  OF  FLIGHT 

FORTRAN  MAIN 

NONE 

T,  USI ,  USF,  UVS,  MVS, MVR,  AT, RMIR, VMIR 
USD, AC, WASTAN, VRATIO, VELWD 
US 


C 

UPDATES  : 

T. 

THORNTON 

- 

CR 

# 

005 

C 

T. 

THORNTON 

- 

CR 

* 

016 

c 

T. 

THORNTON 

- 

CR 

# 

025 

c 

B. 

HILL 

- 

CR 

* 

030 

c 

T. 

THORNTON 

~ 

CR 

# 

037 

c 

T. 

THORNTON 

- 

CR 

* 

042 

c 

T. 

THORNTON 

- 

CR 

* 

046 

c 

D. 

SMITH 

- 

CR 

# 

059 

c 

D. 

SMITH 

- 

CR 

i 

072 

c 

D. 

SMITH 

- 

CR 

# 

073 

c 

B. 

HILL  / 

- 

CR 

* 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH 

- 

CR 

# 

092 

c 

B. 

HILL 

- 

CR 

* 

093 

C 


IMPLICIT  REAL 

(A-H) 

IMPLICIT  REAL 

(O-Z) 

REAL 

USF (3) 

,  UVS (3) 

,  DBAR ( 3 i 

REAL 

USD (3) 

,  AT ( 3 ) 

,  US  (3 ) 

REAL 

AC  (3) 

,  BBAR ( 3 ) 

,  BIGAC (3) 

REAL 

BIGB (3) 

,  MVS 

,  MVR 

REAL 

KS1 

,  KB 

,  M3IGAC 

REAL 

MBIGB 

,  VMIR ( 3 ) 

,  USI (3) 

REAL 

RMIR (3) 

,  GRESTO 

LOCAL 

DATA  USED 

FOR  CONSTANTS, 

VARIABLES  AND  INITIALIZATION  FLA: 

SAVE 

IBSTR 

*  DATA  INITIALIZATION 
$  INCLUDE ( ' “ / INCLUDE/SSCON3  6 . DAT ' ) 
S INCLUDE (’ VINCLUDE/SSCON38.DAT' ) 
S INCLUDE ( ' '/INCLUDE / SSCON39.DAT’ ) 
S INCLUDE (  "/ INCLUDE /SSCON 4  0. DAT'  ) 
SINCLUDE { ''/ INCLUDE /S3CON 30 . DAT ' ) 


n  o 
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DATA  IBSTR  /  1  / 

IF  (  IBSTR. EQ.l  )  THEN 
IBSTR  =  0 

INITIALIZE  FLAG  WHICH  ENABLES  RESET  OF  STEERING  VICTOR  AT 
FRACS  INITIATION 

IF  (  T.GT.TFRCS  )  THEN 
ISETUS  =  1 

ELSEIF  (  T.LE.TFRCS  )  THEN 
ISETUS  =  0 
ENDIF 

ENDIF 


C  LAUNCH  STEERING  LOGIC 

IF  (  T.LE.TC  )  THEN 
USD ( 1 )  =  0.0 
USD (2)  =  C.O 
USD(3)  =  C.C 
AC ( I )  -  US (1 ) 

AC  (2)  =  US  (2) 

AC  ( 3 )  =  US  (3  ) 

C  MINIMUM  IMPULSE  STEERING  (MINS)  LOGIC 

ELSEIF  (  T.LE.T5  )  THEN 

C  RESET  UNIT  STEERING  VECTOR  AT  FRACS  INITIATION 

IF  (  T.GE.TFRCS  .AND.  ISETUS. EQ .  0  )  THEN 

TMPI  =  SORT  (  VMIR (1 ) **2  +  7MIR(2>**2  +  VMIR(3)*'2  > 

US (1)  =  VMIR(l) /TMPI 

US (2)  =  VMIR ( 2 ) /TMPI 

US (3)  =  VMIR (3) /TMPI 

ISETUS  =  1 
ENDIF 

C  CALCULATE  STEERING  VECTOR  DERIVATIVE 

IF  (  T.GE.TFRCS  )  THEN 

C  ESTIMATE  GRAVITY  VECTOR 

TMPI  -  SQRT  (  RMIR ( 1 ) *  *  2  +  RMIR<2)**2  +  RMIR(3)**2  ) 

TMP3  =  TMPI *  *  3 

GREST(l)  =  -  GMU'RMIR (1 ) /TMP3 

GREST ( 2 )  =  -  GMU*RMIR (2) /TMP3 

GREST ( 3 )  =  -  GMU*RMIR (3) /TMP3 

C  ESTIMATE  TURNING  RATE  DUE  TO  GRAVITY 

TMP4  =  SQRT  (  VMIR ( 1 ) *  *  2  +  VMIR(2>**2  -  VMIR(3)**2  ) 

TMPI  =  (  GREST (2) *US (3)  -  GREST (3) *US (2)  I/TMP4 

TMP2  =  (  GREST (3) *US (1)  -  GREST ( 1 ) *US ( 3)  ) /TMP 4 

TMP3  =  (  GREST  (1)  *US  (2)  -  GREST  (2)  *US  (1 )  )/TMP4 

USD ( 1 )  -  US ( 2 ) *TMP3  -  US(3)*TMP2 
USD (2)  =  US (3) *TMP1  -  US(1)*TMP3 
USD ( 3 )  =  US (1 ) *TMP2  -  US(2)*TMPI 
ELSE 

USDOT  *  US  ( 1 )  *  USI  <  1 )  +  US  (2  )  *USI  (2 )  +  US(3)*USI(3) 

USD ( 1 )  =  KS1 * (USI ( 1 )  -  USDOT*US(l)) 

USD ( 2 )  =  KS1*(USI(2)  -  USDOT*US (2 ) ) 

USD ( 3 )  =  KS1 * (USI ( 3 )  -  USDOT*US (3) ) 

ENDIF 


AC ( 1 )  =  US ( 1 ) 

AC  (2)  =  US  (2) 

AC  (3)  =  US  (3) 

GENERAL  ENERGY  MANAGEMENT  (GEMS)  STEERING  LOGIC 

ELSEIF  (  T.LE.TCD  )  THEN 
USD (I)  =  0.0 

USD ( 2 )  =  0.0 

USD (3)  =0.0 

US(1)  =  UVS(l) 

US  (2!  =  UVSI2) 
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US  (  3 )  =  UVS  (3) 

BIGB(l)  =  DBAR(2) *US (3)  -  DBAR ( 3 ) *US ( 2 ) 

BIGB  ( 2 )  =  DBAR  (3)  *US  (1)  -  DBAR (1 ) *US ( 3 ) 

BIGB ( 3 )  =  DBAR (1) *US (2)  -  DBAR ( 2) *US ( 1 ) 

MBIGB  =  SORT (BIGB ( 1 ) *  *2  +  BIGB(2)**2  +  BIGB(3)**2) 
BBAR(l)  =  BIGB  (1) /MBIGB 
BBAR ( 2 )  =  BIGB<2) /MBIGB 
BBAR ( 3 )  =  BIGB(3) /MBIGB 

IF  (  MVR.NE.0.0  )  THEN 
VRATIO  =  MVS/MVR 
ENDIF 

IF  (  MVS.LE.MVR  )  THEN 

WASTAN  =  KB* {1.0  -  VRATIO) **0.5 
ELSE 

WASTAN  =0.0 
ENDIF 

SINWAN  =  VRATIO*  WASTAN 

COSWAN  =  1.0  -  WASTAN* *2/2 . 0 

BIGAC(l)  =  US (1 ) *COSWAN  -  BBAR ( 1 ) *SINW* N 
BIGAC (2)  =  US(2)*COSWAN  -  BBAR (2) 'SINWAN 
BIGAC ( 3 )  =  US (3) 'COSWAN  -  BBAR (3) 'SINWAN 

MB  I G  AC  =  SQRT  (BIGAC  (1)  "2  +  BIGAC  (2)  **2  +  BIGAC(3)**2) 

AC ( 1 )  =  BIGAC (1) /MBIGAC 

AC (2)  =  BIGAC (2) /MBIGAC 

AC (3)  =  BIGAC (3) /MBIGAC 

ATDTUS  =  AT  (1)  *  US  (1)  +  AT(2)*US(2)  +  AT(3)*US(3) 

VELWD  =  SQRT  (AT  (1)  "2  +  AT(2)**2  +  AT(3)**2)  -  ATDTUS 

C  COUNTDOWN  STEERING  LOGIC 

ELSE 

AC ( 1 )  =  US(1) 

AC  (2)  =  US  (2) 

AC (3)  =  US ( 3 ) 

VELWD  =0.0 
ENDIF 

RETURN 

END 


FILE:  uuv22 . 19g/sut i li ty/uubthrst . for 


c - 

SUBROUTINE  BTHRST (T, CG, EISP, PRESS, DLP, DLY,  TOSEED,  TBRK,  IBTHR,  FXT, 
FYT, FZT, MXT, MYT, MZT, MDOTT, THRV, THR) 

C - 

c 

C  SUBROUTINE  NAME  :  BTHRST 

C 

C  AUTHOR (S)  :  L.  D.  HUEBNER,  D.  C.  FOREMAN 

C 

C  FUNCTION  :  COMPUTES  MISSILE  THRUST  VECTOR  AND  MOMENTS 

C  DUE  TO  FIRST  AND  SECOND  STAGE  BOOSTERS 

C 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  TABLE 

C 

C  INPUTS  :  T,CG,EISP, PRESS, DLP, DLY, IBTHR 

C 

C  OUTPUTS  :  FXT, FYT, FZT, MXT, MYT, MZT, MDOTT, THRV, THR 

C 

C  BOTH  :  TOSEED, TBRK 

C 


c 

UPDATES  : 

T. 

THORNTON 

-  CR 

# 

002 

c 

T. 

THORNTON 

-  CR 

* 

016 

c 

D. 

SMITH 

-  CR 

# 

027 

c 

T. 

THORNTON 

-  CR 

# 

037 

c 

B. 

HILL 

-  CR 

# 

038 

c 

D. 

SMITH 

-  CR 

* 

037 

c 

T. 

THORNTON 

-  CR 

* 

043 

c 

T. 

THORNTON 

-  CR 

» 

046 
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c 

D. 

SMITH 

-  CR 

» 

059 

c 

D. 

SMITH 

-  CR 

* 

076 

c 

D. 

SMITH 

-  CR 

* 

080 

c 

B. 

HILL  / 

-  CR 

* 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

-  CR 

1 

087 

c 

B. 

HILL 

-  CR 

# 

089 

c 

D. 

SMITH 

-  CR 

* 

092 

c 

B. 

HILL 

-  CR 

# 

093 

c 

c 


IMPLICIT  REAL 
IMPLICIT  REAL 

REAL  TIMTH1 (26) 
REAL  THRTH2 (29) 
REAL  MXT 
REAL  MDOTT 

INTEGER* 4 


(A-H) 

(0-Z) 

,  THRTH1  (26) 
,  CG { 3 ) 

,  MYT 
,  B0FF1 (2) 

TOSEED 


,  TIMTH2<29) 
,  THRMA (9) 

,  MZT 

,  B0FF2  (2) 


C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  ITH1  ,  ITH2  ,  AEXIT  ,  XNOZ  ,  THRMA 

*  DATA  INITIALIZATION 

SINCLUDE ( 1  * /INCLUDE /SSBTHRST . DAT • ) 

SINCLUDE ( ' A / INCLUDE /SSCON 17 . DAT • ) 

SINCLUDE ( ' */ INCLUDE /SSCON22 . DAT ' ) 

SINCLUDE ( • VINCLUDE/SSCON23.DAT* ) 

SINCLUDE (■ “ /INCLUDE/SSCON29. DAT' ) 

SINCLUDE ( ■ '/INCLUDE/SSCON31 . DAT ' ) 

SINCLUDE  ( 1  VINCLUDE/SSCONSZ  .  DAT  ' ) 

SINCLUDE ( • * /INCLUDE /SSCON41 . DAT • ) 

DATA  ITH1.ITH2  /  2*1  / 

IF  ( IBTHR  ,EQ.  1)  THEN 

IBTHR  =  0 


IF  (T  .LT.  TSTG1 )  THEN 
AEXIT  =  AEXIT1 
XNOZ  «  XNOZ 1 
IF  (T  .EQ.  0.0) 
BOFF2 ( 1 )  -  2 
BOFF2 (2)  =  2 
ENDIF 


THEN 

0*PI*RAN0 (TOSEED) 
0*PI*RAN0 (TOSEED) 


OFF1 
OFF2 
ELSE 

AEXIT 
XNOZ 
OFF1 
OFF  2 
ENDIF 
COFF1 
SOFF1 
COFF2 
SOFF2 


=  BOFF1 (1) 
-  BOFF2 ( 1 ) 

=  AEXIT2 
=  XNOZ2 
-  BOFF1 (2) 

=  BOFF2 (2) 

COS (OFF1 ) 
SIN(OFFl) 
COS (OFF2) 
SIN (OFF2) 


C  CALCULATE  DIRECTION  OF  BOOSTER  NOZZLE  MISALIGNMENT 


THRMA (1) 
THRMA (2) 
THRMA (3) 
THRMA (4) 
THRMA (5) 
THRMA  (6) 
THRMA (7) 
THRMA (8) 
THRMA (9) 
ENDIF 


COFF1 

SOFFl*COFF2 
SOFF1 *SOFF2 
SOFF1 *SOFF2 
COFF1 

SOFF1 *COFF2 
SOFF1 *COFF2 
SOFFl*SOFF2 
COFF1 


IF  (T  .LT.  TSTG2 )  THEN 
C  SFCOND  STAGE  BOOST 


IF  (  T.GE.TSTG1  )  THEN 
TO  =  T  -  TST20N 
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CALL  TABLE (TIMTH2 , THRTH2 , TO , THRV, 29, ITH2) 

C  FIRST  STAGE  BOOST 

ELSE 

TO  =  T  -  TIGN 

CALL  TABLE (TIMTH1, THRTH1, TO, THRV, 26, ITH1) 

ENDIF 

C  COMPUTE  DELIVERED  THRUST 

THR  =  AMAX1  (  0.0  ,  THRV  -  AEXIT'PRESS  ) 

C  RESOLVE  DELIVERED  THRUST  FROM  GIMBAL  TO  BODY  COORDINATES 

FX  =  THR'COS (DLP) "COS (DLY) 

FY  =  THR'SIN (DLY) 

FZ  =  -  THR*SIN (DLP) 'COS (DLY) 

C  INCORPORATE  THRUSTER  MISALIGNMENTS 

FXT  =  FX'THRMA (1 )  +  FY'THRMA ( 4 )  +  FZ'THRMA ( 1 ) 

FYT  =  FX'THRMA (2 )  +  FY'THRMA (5)  +  FZ'THRMA (8) 

FZT  =  FX'THRMA (3)  +  FY'THRMA (6)  +  FZ'THRMA (9) 

C  CALCULATE  THE  MOMENTS  DUE  TO  THRUST 

MXT  =  FYT'CG ( 3 )  -  FZT*CG(2) 

MYT  =  -  FXT'CG  ( 3 )  +  FZT*(CG(1)  -  XNOZ) 

MZT  =  FXT'CG (2)  -  FYT*(CG(1)  -  XNOZ) 

C  CALCUIATE  MASS  EXPULSION  RATE 

MDOTT  =  THRV/EISP 

ELSE 


FXT 

0.0 

FYT 

= 

0.0 

FZT 

0.0 

MXT 

= 

0.0 

MYT 

= 

0.0 

MZT 

= 

0.0 

MDOTT 

= 

0.0 

THR 

0.0 

THRV 

= 

0.0 

ENDIF 

RETURN 

END 


FILE :  uuv22 .19g/sutility/uubxi2fv.for 


SUBROUTINE  BXI2FV  (  FVM,  B,  FV  ) 


SUBROUTINE  NAME  :  BXI2FV 

AUTHOR (S)  :  W.  E.  EXELY 

FUNCTION  :  COMPUTE  QUATERNION  (FV)  ATTITUDE  PARAMETERS 

FROM  A  BODY  TO  INERTIAL  TRANSFORMATION 
MATRIX  IB)  AND  SET  THE  SQUARE  OF  MAGNITUDE 
OF  QUATERNION  TO  (FVM) 

CALLED  FROM  :  FORTRAN  MAIN 

SUBROUTINES  CALLED  :  NONE 

INPUTS  :  FVM,  B 

OUTPUTS  :  FV 


UPDATES  : 


D.  SMITH 


CR  #  59 


nnn  no  o  n  n  n  nn  nn  nn  n  non 
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IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 

C 

DIMENSION  B  (  9  )  ,  FV  (  4  ) 

C 

EQUIVALENCE  (  T3  ,  Q  ) , (  B1  ,  AA  ) 

C 

DATA  F  4 ,  F2,  P25,  P0001  /  4.,  2.,  0.25,  0.0001  / 

DATA  FI,  F0  /  1.,  0.  / 

C 

T3  =  P25 
C 

A1  =  B  <  6)  -  B  ( 8 ) 

A2  =  B(7)  -  B  < 3 ) 

A3  =  B  (2)  -  B  < 4  > 

C 

TRA  =  B  (1 )  +  B  ( 5 )  +  B(9)  +  FI 
C 

IF  (  TRA  .LT.  P0001  )  GO  TO  100 
C 

FV<4)  -  SORT (T3*TRA) 

T3  -  T3/FV { 4 ) 

FV  (1)  =  T3*A1 
FV (2)  -  T3*A2 
FV (3)  -  T3*A3 
GO  TO  200 
C 

100  CONTINUE 

SETUP  FOR  ILL-CONDITION  ...  TRA  =  0  (LT  .0001) 

IFLAG  -  0 
TRA  =  F2  -  TRA 

B1  =  T3* (  B(l)  +  B (1 )  +  TRA  ) 

IF  (  B1  .LT.  F0  )  B1  -  F0 
FV ( 1 )  »  SQRT (  B1  ) 

IF (  FV (1)  .NE.  F0  )  IFLAG  «  1 

B1  =  T3*  (  B(5)  +  B  ( 5 )  +  TRA  ) 

IF  (  B1  .LT.  tO  )  B1  =  F0 
FV  (2)  »  SQRT (  B1  ) 

IF  (  IFLAG  .EQ.  1  )  FV(2)  =  SIGN  (  FV(2>,  B(2)+B(4)  ) 

B1  -  T3«(  B  ( 9)  +  B(9)  f  TRA  ) 

IF  <  B1  .LT.  F0  >  B1  F0 
FV (3)  «  SQRT (  B1  ) 

IF  (  IFLAG  .EQ.  1  )  FV(3)  =  SIGN  (  FV(3),  B(3)+B(7)  ) 

IF (  IFLAG  .EQ.  1  )  GO  TO  110 

IF  (  FV  (2)  .NE.  F0  )  FV(3)  =  SIGN  (  FV<3>,  B(6)+B(8)  ) 

110  CONTINUE 

AA  *  F0 
FV  (4)  *  F0 
Q  =  F0 

IF (  FV ( 1 )  .EQ.  F0  )  GO  TO  120 
Q  -  F4 

AA  -  AA  +  Al/FVU) 

120  CONTINUE 

IF (  FV (2)  .EQ.  F0  )  GO  TO  140 
Q  -  Q  +  F4 
AA  =  AA  +  A2/FV (2) 

140  CONTINUE 

IF (  FV ( 3 )  .EQ.  F0  )  GO  TO  160 
Q  =  Q  +  F4 
AA  =  AA  +  A3/FV (3) 

160  CONTINUE 

IF (  Q  .NE.  F0  )  FV ( 4 )  =  AA/Q 
200  CONTINUE 

RE-NORMALIZE  QUATERNION 


19.  Appendix  N:  EXOSIM  2.0  (End-to-end) 


149 


Q  =  SQRT  (  FV(1)**2  +  FV(2)**2  +  FV(3)**2  +  FV(4)**2  ) 
IF {  Q  .EQ.  F0  )  GO  TO  500 
Q  =  FVM/Q 
C 

FV  (1 )  =  Q*FV(1) 

FV  (2)  -  Q*FV(2) 

FV ( 3 )  =  Q*FV(3) 

FV{4)  -  Q*FV<4) 

C 

500  CONTINUE 
C 

RETURN 

END 


FILE :  uuv22 . 1 9g/sut ility /uucorvel .for 


SUBROUTINE 

CORVEL (T, MVR, VTT, RMIR, VMIR, VTTP , VG, VS, MVS, UVS, VC, DLV, 
TFFE, TTFE) 

c 

c 

c 

SUBROUTINE 

NAME  :  CORVEL 

c 

c 

AUTHOR (S)  : 

M.  K.  DOUBLEDAY,  L.  C.  HECK 

c 

c 

FUNCTION  : 

CALCULATES  THE  CORRELATED  VELOCITY 

c 

CALLED  FROM 

:  FORTRAN  MAIN 

c 

c 

SUBROUTINES 

CALLED  NONE 

c 

c 

INPUTS  : 

T, MVR, VTT, RMIR, VMIR 

c 

c 

OUTPUTS  : 

VS,  MVS,  UVS, VC, DLV, TFFE, TTFE 

c 

c 

BOTH  : 

VTTP, VG 

c 

UPDATES  : 

T.  THORNTON  -  CR  #  025 

c 

D.  SMITH  -  CR  #  0X3 

c 

B.  HILL  -  CR  #  030 

c 

T.  THORNTON  -  CR  #  033 

c 

T.  THORNTON  -  CR  1  042 

c 

T.  THORNTON  -  CR  #  043 

c 

T.  THORNTON  -  CR  #  044 

c 

U.  SMITH  -  CR  #  059 

c 

D.  SMITH  -  CR  #  072 

c 

B.  HILL  ,  -  CR  #  081 

c 

R.  RHYNE 

c 

c 

r — 

B.  HILL  -  CR  #  093 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 


REAL 

DLV (3) 

,  MDVT 

REAL 

MRT 

,  MTMPV 

REAL 

MVR 

,  MVS 

REAL 

RMIR ( 3  J 

REAL 

RB  (3) 

,  RTPRED (3) 

REAL 

TMPV  ( 3 ) 

,  URBI3) 

REAL 

UTHP (3) 

,  UTMPV ( 3  > 

REAL 

VC  (3) 

,  VCE ( 3 ) 

REAL 

VD0  (3) 

,  VG  ( 3 ) 

REAL 

VMIR (3) 

,  VPHI  (  3 ) 

REAL 

VSE  (3) 

,  VTT (3) 

C  LOCAL  DATA  USED  FOR  CONSTANTS, 


,  MRB 
,  MVCE 
,  MVSE 


,  URT(3) 

,  UVS ( 3 ) 

,  VGE (3) 

,  VS ( 3 ) 

,  VTTP {3 ) 

VARIABLES  AND  INITIALIZATION  FLAG 


SAVE 


ICORV 


*  DATA  INITIALIZATION 
SINCLUDE ( • A/INCLUDE/SSCON39.DAT' ) 
S INCLUDE ( ' * /INCLUDE/ SSCON 42 . DAT ' ) 
SINCLUDE ( ' ^ /INCLUDE/SSCON4  3 . DAT ' ) 
SINCLUDE ( ' ^ / INCLUDE /SSCON2 3 . DAT  1 ) 


DATA  ICORV  /  1  / 


n  o 
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IF  ( ICORV  ,£Q.  1)  THEN 
ICORV  =  0 

IF  (T  .EQ.  0.0)  THEN 
I LOOP  =  50 
ELSE 

I LOOP  =  1 
ENDIF 
ELSE 

I LOOP  =  1 
ENDIF 

ESTIMATE  VELOCITY  TO  BE  GAINED  (VGE)  ,  CORRELATED  VELOCITY  (VCE) 
AND  STEERING  VELOCITY  (VSE) 

DO  10  1=1,3 

DLV(I)  =  VTT(I)  -  VTTP  (I) 

VGE (I)  =  VG  (I)  -  DLV(I) 

VCE  (I)  =  VGE (I)  +  VMIR(I) 

VSE (I)  «  VGE  (I)  -  VD0 (I) 

VTTP (I)  =  VTT  (I) 

10  CONTINUE 

MVSE  =  SQRT  (  VSE (1) **2  +  VSE(2)**2  +  VSE(3>**2  ) 

MDVT  =  SQRT  (  DLV(1)**2  +  DLV(2)**2  +  DLV(3)**2  ) 

C  CALCULATE  POSITION  BIAS  SCALE  FACTOR 

IF  (  MVSE.GT.MVR  )  THEN 
SCALE 3  =  MVR/MVSE 
ELSE 

SCALE3  =1.0 
END  IF 

SCALAR  =  F2  *  MVR  *  SCALE3  /  (  FI  +  MDVT  ) 

C  CALCULATE  OFFSET  POSITION  VECTOR 

IF  (  T.GE.TSTG2  )  THEN 
RB  (1)  =  RMIR(l) 

RB (2)  *  RMIR (2) 

RB  (3)  =  RMIR  (3) 

ELSE 

RB(1)  =  RMIR ( 1 )  +  SCALAR* VSE ( 1 ) 

RB(2)  =  RMIR (2)  +  SCALAR*VSE (2) 

RB (3)  =  RMIR (3)  +  SCALAR*VSE ( 3 ) 

END  IF 

DO  30  I  =  1, I LOOP 

C  COMPUTE  UNIT  VECTORS 

MRB  =  SQRT (RB ( 1 ) **2  +  RB<2)**2  +  RB<3)**2) 

URB(l)  =  RB (1 ) /MRB 
URB (2)  -  RB (2) /MRB 
URB ( 3 )  =  RB ( 3 ) /MRB 

MRT  =  SQRT  (RTF-RED  (1)  **2  +  RTPRED(2>**2  +  RTPRED  (3)  **2) 

URT(l)  =  RTPRED (1) /MRT 
URT (2)  =  RTPRED (2) /MRT 
URT ( 3 )  =  RTPRED (3) /MRT 

TMPV(l)  =  URB (Zi A URT ( 3 )  -  URB (?) *URT (2) 

TMPV (2 )  =  URB (3) *URT (1)  -  URB (1 ) *URT ( 3) 

TMPV ( 3 )  =  URB (1) *URT (2)  -  URB ( 2 ) *URT ( 1 ) 

MTMPV  =  SQRT ( TMPV ( 1 ) *  *  2  +  TMPV(2)**2  +  TMPV(3)**2) 

UTMPV(l)  =  TMPV (1) /MTMPV 
UTMPV (2)  =  TMPV (2) /MTMPV 
UTMPV (3)  =  TMPV (3) /MTMPV 

UTHP(l)  =  UTMPV ( 2  >  *  URB ( 3 )  -  UTMPV (3) *URB (2) 

UTHP ( 2 )  =  UTMPV ( 3 ) *  URB ( 1 )  -  UTMPV ( 1 ) *URB ( 3 ) 

UTHPI3)  =  UTMPV ( 1 ) *URB ( 2 )  -  UTMPV (2) *URB (1 ) 

C  ESTIMATE  HORIZONTAL  AND  RADIAL  COMPONENTS  OF  VC 

VHC  =  VCE (1) *UTHP (1)  +  VCE (2) *UTHP (2)  +  VCE ( 3 ) *UTHP ( 3 ) 

VCR  =  VCE  (1)  *URB  (1)  +  VCE  (2)  *URB  (2)  +  VCE  (3)  *URB  (3) 
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C  COMPUTE  SIN  AND  COS  OF  RANGE  ANGLE 

VP HI(1)  =  URB<2) *URT<3)  -  URB (3) *URT (2) 

VPHI (2)  =  URB<3) *URT(1)  -  URB ( 1 ) *URT ( 3) 

VPHI(3)  =  URB(l)  *URT(2)  -  URB  ( 2 )  *URT  (1 ) 

SINPHI  =  SQRT  (  VPHI  ( 1 )  * *2  +  VPHI  (2)  "2  +  VPHI<3)**2  ) 

COSPHI  =  URB(1)*URT(1)  +  URB (2) *URT (2)  +  URB (3 ) *URT ( 3 ) 

C  COMPUTE  INTERMEDIATE  VARIABLES 

MVCE  =  SQRT  (  VCE(1)"2  +  VCE(2)'”2  +  VCE(3)**2  ) 

W  =  VHC  /  MRB 

EL  =  MRB  *  VHC* '2  /  GMU 

AR  =  MRB  /  MRT 

TP1  =  MVCE**2  *  MRB  /  GMU 

HHH  =  EL  *  SINPHI**2  *  (  2.0  -  TP1  ) 

SQRHHH  =  SQRT  (  HHH  ) 

C  COMPUTE  TIME  OF  FLIGHT  ESTIMATE 

T1  =  EL  *  SINPHI  /  (  HHH  *  W  ) 

T2A  =  (1.0-  EL)  /AR+1.0-  AR'EL 
T2B  =  (  2 . 0*EL  -  1.0  -  1.0/AR  )  *  COSPHI 
T2  -  T2A  +  T2B 

T3  =  2.0  '  EL**2  *  SINPHI**3  /  (  W  *  HHH  *  SQRHHH  ) 

T4A  =  SQRHHH 

T4B  =  EL  +  AR'EL  +  COSPHI  -  1.0 

T4  =  ATAN2 (  T4 A  ,  T4B  ) 

TFFE  «  T1*T2  +  T3*T4 

C  ESTIMATE  TOTAL  TIME  OF  FLIGHT 

TTFE  =  T  +  TFFE 

C  COMPUTE  TIME  OF  FREE  FALL  AND  TIME  OF  FLIGHT  ERROR 

TFF  =  TTF  -  T 

DELTF  =  TFF  -  TFFE 

C  COMPUTE  PARTIAL  OF  TFF  W/RESPECT  TO  VC 

A  =  2.0  '  <  AR  -  COSPHI  )  /  SINPHI  +  (  VCR  /  VHC  ) 

B  =  A'VCR  -  VHC 

C  =  B  *  MRB  /  GMU 

D  =  C  *  EL  '  SINPHI *  *2 

E  =  D  +  HHH/VHC 

PARHV  =  E  *  2.0 

PART1V  =  (  1.0/VHC  -  PARHV/HHH  )  *  T1 

PART2V  =  (  2.0*EL/VHC  )  *  (  2.0*COSPHI  -  (1 . 0+AR"2) /AR  ) 

PART3V  =  (  1.0/VHC  -  PARHV/ ( 2 . O'HHH)  )  *  3.0  *  T3 

SUBEQ1  =  (  EL  +  AR'EL  +  COSPHI  -  1.0  )  *  VHC  *  PARHV 

SUBEQ2  =  4.0  *  HHH  *  EL  *  <  1 . 0  +  AR  ) 

SUBEQ3  =  (  EL  +  AR'EL  +  COSPHI-l.O  )  "2  +  HHH 
SUBEQ4  -  2.0  *  SQRHHH  »  VHC 

PART 4 V  =  (  SUBEQ1  -  SUBEQ2  )  /  (  SUBEQ3  *  SUBEQ4  ) 

PTFFV  =  T1 'PART2V  +  T2*PART1V  +  T3*PART4V  +  T4*PART3V 

VCOPK  =  VHC  +  DELTF/PTFFV 

C  COMPUTE  CORRELATED  VELOCITY  VECTOR 

C  HIT  EQUATION  FOR  RADIAL  COMPONENT  OF  VCP 

VCRPK  =  VCOPK/ (EL'SINPHI)  '  (  1.0  -  AR'EL  -  (1 . 0-EL) 'COSPHI  ) 

C  COMPUTE  VC, VG, VS 

DO  20  J  -  1  ,  3 

VC(J)  =  VCRPK'URB ( J)  +  VCOPK'UTHP (J) 

VG  ( J)  =  VC(J)  -  VMIR(J) 

VS  (J)  =  VG  (J)  -  VD0  (J) 

20  CONTINUE 

30  CONTINUE 

MVS  -  SQRT  (VS  ( 1 )  "2  +  VS(2)"2  +  VS(3>"2) 
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UVS  (1)  =  VS  (1)  /MVS 
UVS  (2)  -  VS  (2)  /MVS 
UVS  (3)  -  VS  (3)  /MVS 

RETURN 

END 


FILE:  uuv22 . 19g/sutility/uucw87 .for 


subroutine  cw87 

integer*2  icw87 

call  stcw87 (icw87) 

icw87  *  icw87  .and.  #ff7ah 

call  ldcw87 (icw87) 

end 


FILE:  uuv22.19g/sutility/uufracs. for 


C - 

SUBROUTINE  FRACS (T, DLPC, DLYC, VCMD, VLVCMS) 

c - 

c 

C  SUBROUTINE  NAME  :  FRACS 

C 

C  AUTHOR (S)  :  DAVID  F.  SMITH 

C 

C  FUNCTION  :  GENERATE  THRUSTER  VALVE  COMMANDS 

V, 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  TABLE 

C 

C  INPUTS  :  T, DLPC, DLYC 

C 

C  OUTPUTS  :  VCMD, VLVCM5 

C 


c 

UPDATES  : 

B. 

HILL 

- 

CR 

# 

008 

c 

B. 

HILL 

- 

CR 

# 

022 

c 

T. 

THORNTON 

- 

CR 

# 

026 

c 

B. 

HILL 

- 

CR 

# 

030 

c 

T. 

THORNTON 

- 

CR 

* 

037 

c 

B. 

HILL 

- 

CR 

1 

038 

c 

B. 

HILL 

- 

CR 

* 

046 

c 

B. 

HILL 

- 

CR 

* 

054 

c 

D. 

SMITH 

- 

CR 

# 

059 

c 

D. 

SISSOM 

- 

CR 

# 

069 

c 

D. 

SMITH 

- 

CR 

# 

072 

c 

B. 

HILL  / 

- 

CR 

* 

081 

c 

R. 

RHYNE 

c 

B. 

HILL 

- 

CR 

1 

086 

c 

D. 

SMITH 

- 

CR 

# 

092 

c 

B. 

HILL 

- 

CR 

* 

093 

C 

C - 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 

REAL  VCMD ( 4 ) 

INTEGER  VLVCM5 

*  DATA  INITIALIZATION 

SINCLUDE ( ' */ INCLUDE /SSCON4  9 . DAT ' ) 

S INCLUDE ( ' A / INCLUDE /SSC0N51 . DAT ' ) 

C  DETERMINE  PITCH  PLANE  VALVE  COMMANDS 

IF  (  DLPC. GE. DELON  )  THEN 
VCMD (2)  -  0.0 
VCMD (4)  -  DTFRU'DLPC 

ELSEIF  (  DLPC  .  T,T .  DELCN  .AND.  DLPC .  GT . -DELON  )  THEN 
VCMD (2)  =  0.0 
VCMD (4)  =0.0 

ELSEIF  (  DLPC. LE. -DELON  )  THEN 
VCMD (2)  =  DTFRU'ABS (DLPC) 

VCMD ( 4 )  =0.0 
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ENDIF 

C  DETERMINE  YAW  PLANE  VALVE  COMMANDS 

IF  (  DLYC.GE. DELON  )  THEN 
VCMD{1)  =0.0 
VCMD (3)  =  DTFRU'DLYC 

ELSEIF  (  DLYC.LT. DELON  .AND.  DLYC . GT . -DELON  )  THEN 
VCMD(l)  =0.0 
VCMD (3)  =  0.0 

ELSEIF  (  DLYC. LE. -DELON  )  THEN 
VCMD(l)  =  DTFRU*ABS (DLYC) 

VCMD (3)  =0.0 

ENDIF 

C  UPDATE  TOTAL  NUMBER  OF  CYCLES  THAT  THE  VALVES  ARE  ON 

DO  10  I  =  1  ,  4 

IF  (  VCMD (I) .NE. 0.0  )  THEN 
VLVCM5  =  VLVCM5  +  1 
ENDIF 
10  CONTINUE 

RETURN 

END 


FILE:  uuv22 . 19g/ suCility/uuf ret hr . for 


c - 

SUBROUTINE  FRCTHR (T, CG, MACH. QA, VCMD, VCMDL, DTOFF, TFTAB, IFTAB, 

TOSEED , TBRK , TMF , THF , LENF , FRCX , FRCY , FRCZ , MRCX , 
MRCY, MRCZ, MDOTF, ATHRF, KN,  KM,  FOFF1 ,  FOFF2 ) 


c — 
c 

c 

SUBROUTINE  NAME  : 

FRCTHR 

c 

AUTHOR (S)  . 

K. 

S.  BOGAN,  D.  C 

FOREMAN 

c 

c 

FUNCTION  : 

COMPUTES  FORCES  AND  MOMENTS  RESULTING  FROM 

c 

THE  FORWARD  REACTION  CONTROL  THRUSTERS 

c 

CALLED  FROM  : 

FORTRAN  MAIN 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

INPUTS  : 

T, 

CG, MACH, QA, VCMD 

I,  VCMDL,  DTOFF,  TFTAB 

c 

OUTPUTS  : 

FRCX, FRCY, FRCZ, MRCX,  MRCY, MRCZ, MDOTF, ATHRF, 

c 

KN 

, KM, FOFF1 , FOFF2 

c 

BOTH  : 

IFTAB, TOSEED, TBRK 

' 

TMF, THF, LENF 

c 

UPDATES  : 

B. 

HILL  -  CR 

# 

008 

c 

B. 

HILL  -  CR 

022 

c 

T. 

THORNTON  -  CR 

# 

026 

c 

B. 

HILL  -  CR 

# 

030 

c 

B. 

HILL  -  CR 

# 

038 

c 

B. 

HILL  -  CR 

# 

046 

c 

D. 

SMITH  -  CR 

# 

059 

c 

D. 

SISSOM  -  CR 

# 

061 

c 

D. 

SISSOM  -  CR 

# 

069 

c 

D. 

SMITH  -  CR 

* 

072 

c 

D. 

SMITH  -  CR 

076 

c 

D. 

SMITH  -  CR 

* 

080 

c 

B. 

HILL  /  -  CR 

# 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH  -  CR 

# 

082 

c 

R. 

RHYNE  -  CR 

* 

084 

c 

B. 

HILL  -  CR 

» 

086 

c 

R. 

RHYNE  -  CR 

« 

087 

c 

B. 

HILL  -  CR 

* 

089 

c 

D. 

SMITH  -  CR 

* 

092 

c 

B. 

UTT  T _ -  rn 

i 

0<*3 

C 

c--- 

_ 

_ 

IMPLICIT  REAL 
IMPLICIT  REAL 


(A-H) 

(O-Z) 
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REAL  ERCDIR  (3,4)  ,  FRCLOC(3,4)  ,  FRCMA<9,4) 

REAL  CG(3)  ,  F0(3)  ,  VCMDL(4) 

REAL  F  <  3 )  ,  XMOM ( 3 )  ,  M ( 3 ) 

REAL  MRCX  ,  MRCY  ,  MRC2 

REAL  MACH  ,  MCHLIM  ,  KN 

REAL  KM  ,  LD  ,  MDOTF 

REAL  FISP  ,  TMF(8,4)  ,  THF(8,4) 

REAL  ATHRF ( 4 )  ,  KNFAC  ,  DTOFF ( 4 ) 

REAL  KMFAC  ,  F0FF1 ( 4 )  ,  FOFF2I4) 

REAL  VCMD ( 4 ) 

INTEGER  INDX ( 4 )  ,  LENF ( 4 ) 

INT£G£R*4  TOSEED 

INTEGER  VCOD ( 4 ) 

C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IFRCTH 

-  DATA  INITIALIZATION 

SINCLUDE ( '  "VINCLUDE/SSFRCTHR. DAT  1 ) 

SINCLUDE  ( *  'VINCLUDE/SSCONOl.DAT1  ) 

SINCLUDE ( 1 " /INCLUDE/ SSCON1 7 . DAT  1 ) 

SINCLUDE ( •  -VINCLUDE/SSCON22 . DAT  1 ) 

SINCLUDE ( • "V  INCLUDE /SSCON2 3 . DAT ’ ) 

SINCLUDE ( ■ A/ INCLUDE /SSCON30 , DAT  1 ) 

SINCLUDE ( 1 "/ INCLUDE/ SSCON32 . DAT  1 ) 

SINCLUDE ( 1 '/INCLUDE/SSCON33 . DAT  1 ) 

SINCLUDE ( 1 */ INCLUDE /SSCON4 9 . DAT ' ) 

SINCLUDE ( 1 */ INCLUDE/ SSCON51 . DAT ' ) 

SINCLUDE ( 1 */INCLUDE/SSCON52 . DAT  1 ) 

DATA  IFRCTH  /  1  / 

IF  (  IFRCTH. EQ.l  )  THEN 

IFRCTH  =  0 

IF  (T  .LT.  TFRCS+EPSL)  THEN 

C  FRACS  MISALIGNMENT  DIRECTIONS 

C  FOFF1  -  CONE  ANGLE  OFF  NORMAL 

C  FOFF2  «  POLAR  ANGLE 

CALL  NORM (SDTHR, 0.0, TOSEED, FOFF1 (1) ) 

CALL  NORM (SDTHR, 0.0, TOSEED, FOFF1 (2) ) 

CALL  NORM (SDTHR, 0.0, TOSEED, FOFF1 (3) ) 

CALL  NORM (SDTHR, 0.0, TOSEED,  FOFF1  (4) ) 

FOFF2 ( 1 )  =  2. 0*PI*RAN0 (TOSEED) 

FOFF2 (2)  =  2. 0*PI*RAN0 (TOSEED) 

FOFF2 (3)  =  2. 0*PI*RAN0 (TOSEED) 

FOFF2 ( 4 )  =  2. 0*PI*RAN0 (TOSEED) 

ENDIF 

C  FRACS  THRUSTER  MISALIGNMENT  MATRIX 

DO  1 0  I  *  1  ,  4 

CFOFF1  =  COS (FOFF1  (I)  ) 

SFOFF1  =  SIN (FOFF1  (I)  ) 

CFOFF2  -  COS (FOFF2 (I) ) 

SFOFF2  -  SIN (FOFF2 (I) ) 

FRCMA (1,1)  -  CFOFF1 
FRCMA (2,1)  =  SFOFF1 *CFOFF2 
FRCMA (3, I)  =  SFOFFl*SFOFF2 
FRCMA (4, I)  =  SFOFF1 *SFOFF2 
FRCMA (5,1)  =  CFOFF1 
FRCMA (6,1)  =  SFOFF1 *CFOFF2 
FRCMA  ^,1)  =  SFOFF1 *CFOFF2 
FRCMA (8, I)  =  SFCFF1 *SFOFF2 
FRCMA (9, I)  =  CFOFF1 
10  CONTINUE 

ENDIF 

C  REINITIALIZE  FORCES  AND  MOMENTS  EACH  PASS  THROUGH 


TRCX 

=  0.0 

FRCY 

=■  0.0 

FRCZ 

=  0.0 
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MRCX  =0.0 
MRCY  =0.0 
MRCZ  =0.0 
MDOTF  =0.0 

IF  (T  .LT.  TSTG2)  THEN 

C  CALCULATE  JET  INTERACTION  AMPLIFICATION  FACTORS 


IF  (  T.LE.TSTG1  )  THEN 

LD  =  (  XJET  -  XNOZ1  ) /DJET 

ELSE 

LD  =  (  XJET  -  XNOZ2  ) /DJET 

ENDIF 

CT  =  THJET/ (  QA*SJET  ) 

C  FORCE  COEFFICIENT 

IF  (  MACH .  LE .  MCHLIM  )  THEN 

KN  =  0.6118  +  (0. 1358* <1 .-0.48S*SQRT (LD) ) /SQRT (CT) ) 

+  0 . 094  6*MACH  +  0.004317/LD 

ELSE 

KN  =  1.0  +  EXP (1.1  -  0.2116* (ALOG (CT) +8. 5) **1. 4) 

ENDIF 

C  MOMENT  COEFFICIENT 

KM  =  0.5582  -  0.1884/SQRT(CT)  -  1.9659/LD 
IF  (  IFTAB.EQ. 1  )  THEN 

*  1'tie  ItTAB  assignment  was  moved  to  the  partition  with  FRACS 

*  IFTAB  =  0 

C  LOOP  ON  EACH  VALVE 

DO  20  I  =  1  ,  4 

C  INITIALIZE  TEMPORARY  TABLE  LOOKUP  POINTER  TO  ZERO 

ITMP  =  0 

C  VALVE  COMMAND  IS  OFF 

IF  (  VCMD(I) .EQ.0.0  )  THEN 

C  VALVE  WAS  SHUT  DURING  CYCLE  JUST  ENDED 

IF  (  LENF (I) . EQ . 0  )  THEN 

C  VALVE  WAS  OPEN  DURING  CYCLE  JUST  ENDED 

ELSEIF  (  LENF ( I ) . GT . 0  )  THEN 

C  VALVE  IS  CURRENTLY  SHUT 

IF  (  TFTAB.GE.TMF (LENF (I! , I)  )  THEN 
LENF  (I)  =  0 

C  VALVE  IS  CURRENTLY  OPEN 

ELSEIF  (  TFTAB.LT. TMF (LENF (I) , I)  )  THEN 

C  VALVE  IS  RAMPING  SHUT 

IF  (  TFTAB . GE . TMF ( LENF (I) — 1# I)  )  THEN 

CALL  TABLE (TMF (1, I) , THF (1, I) , TFTAB, TMP1, 
LENF (I) , ITMP) 

TMF (1,1)  =  TFTAB 

TMF  (2,1)  =  TMF  (LENF(I)  ,  I) 

THF (1, I)  =  TMP1 
THF (2, I)  =0.0 
LENF (I)  =  2 

C  VALVE  IS  WIDE  OPEN 

ELSEIF  (  TFTAB.  L,i  .  tMF  (LtNr  ti ) -i ,  l )  )  1  HEN 

TMF (1,1)  =  TFTAB 
TMF (2,1)  =  TMF (LENF (I) -1, I) 


u  u 
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TMF (3, I)  =  TMF  (LENF  (I)  ,  I) 

THF  (1,1)  =  1.0 
THF(2,I)  =  1.0 
THF (3, I)  =  0.0 
LENF  (I)  =  3 

ENDXF 

ENDXF 

ENDXF 

C  VALVE  IS  COMMANDED  OPEN 

ELSEIF  (  VCMD  ( IJ-.GT .0.0  )  THEN 
*  FTN286  X415  OPTIMIZE (3) 

99999  CONTINUE 

C  VALVE  WAS  SHUT  DURING  CYCLE  JUST  ENDED 

IF  (  LENF (I) .EQ. 0  )  THEN 

VCMD ( I )  =  0.001'ANINT (VCMD (I) /0.001) 

TMF(1,I)  =  TFTAB 
TMF(2,I)  =  TMF  (1,1)  +  TLAGFR 
TMF <3, I)  =  TMF(2,I)  +  TRUPFR 
TMF(4,I)  =  TMF(2,I)  +  VCMD(I) 

TMF  (5,  I)  =  TMF (4, I)  +  TRDNFR 

THF (1,1)  =0.0 

THF  (2,1)  =  0.0 

THF (3, I)  =1.0 

THF  (4, I)  =1.0 

THF  (5,1)  =  0.0 

LENF (I)  =  5 

C  VALVE  WAS  OPEN  DURING  CYCLE  JUST  ENDED 

ELSEIF  (  LENF ( I ) . GT . 0  )  THEN 

C  VALVE  IS  CURRENTLY  SHUT 

IF  (  TFTAB. GE.TMF (LENF (I! , I)  )  THEN 

VCMD ( I )  =  0.001*ANINT (VCMD (I) /0.001) 

TMF (1, I)  =  TFTAB 
TMF  (2,1)  =  TMF  (1,1)  +  TLAGFR 
TMF (3, I)  =  TMF (2, I)  +  TRUPFR 
TMF  (4, I)  =  TMF  (2,  I)  +  VCMD  (I) 

TMF  (5, 1)  =  TMF  (4,  I)  +  TRDNFR 

THF  (1,1)  =0.0 

THF  (2,1)  =  0.0 

THF  (3,1)  =  1.0 

THF  (4, I)  =  1.0 

THF (5,1)  =  0.0 

LENF (I)  =  5 

C  VALVE  IS  CURRENTLY  OPEN 

ELSEIF  (  TFTAB.LT. TMF (LENF(I) , I)  )  THEN 

C  VALVE  WILL  BE  SHUT  AT  REISSUE  TIME 

IF  (  TFTAB+TLAGFR. GE.TMF (LENF (I)  ,  I)  )  THEN 

C  VALVE  IS  RAMPING  SHUT  NOW 

IF  (  TFTAB . GT. TMF (LENF ( I ) -1 ,  I )  )  THEN 
TMP1  =  TMF (LENF (I) , I)  -  TFTAB 

VLVRES  =  0.5*TMP1**2/TRDNFR 

VCMD (I )  =  VCMD (I)  -  VLVRES 

CALL  TABLE (TMF (1, I) , THF (1, I) , TFTAB, TMP1, 
LENF (I) , ITMP ) 

rSSUE  A  NEW  COMMAND  IF  THRESHOLD  IS 
REACHED 

IF  (  VCMD (I) .GE. DELON* DTFRU  )  THEN 

VCMD (I)  =  AMIN1  (  DTFRU  ,  VCMD ( I )  ) 

VCMD (I)  =  0.001  * AN I NT (VCMD ( I ) / 

0.001) 

TMF (1,1)  =  TFTAB 
TMF (2,1)  =  TMF (LENF (I) , I) 

TMF (3, I)  =  TMF (1,1)  +  TLAGFR 
TMF (4, I)  =  TMF (3,  I)  +  TRUPFR 
TMF (5, I)  =  TMF (3, I)  +  VCMD (I) 


nn  o  n  on  no  o  on 
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TMF(6,I)  =  TMF(5,I)  *  TRDNFR 

THF  (1,  I)  =  TMP1 

THF(2,I)  =  0.0 

THF  (3, I)  =  0.0 

THF  (4,1)  =  1.0 

THF (5, I)  =1.0 

THF (6,1)  =  0.0 

LENF(I)  =  6 

NO  NEW  COMMAND  IS  ISSUED  IF  THRESHOLD  IS 

NOT  REACHED 

ELSEIF  (  VCMD(I) . LT . DELON*DTFRU  )  THEN 
TMF (1,1)  =  TFTAB 
TMF  (2,1)  =  TMFtLENF  (I)  ,  I) 

THF (1,1)  =  TMP1 
THF (2, I)  =  0.0 
LENF ( I )  =2 

ENDIF 

VALVE  IS  WIDE  OPEN  NOW 

ELSEIF  (  TFTAB. LE. TMF (LENF (I) -1,1)  )  THEN 

VLVRES  =  TMF (LENF (I) -1, I)  -  TFTAB 
+  0.5 'TRDNFR 

VCMD(I)  =  VCMD(I)  -  VLVRES 

ISSUE  A  NEW  COMMAND  IF  THRESHOLD  IS 

REACHED 

IF  (  VCMD ( I ) . GE . DELON  *  DTFRU  )  THEN 

VCMD(I)  =  AMIN1  (  DTFRU  ,  VCMD (I)  ) 
VCMD(I)  =  0 . 001'ANINT (VCMD (I) / 

0.001) 

TMF (1,1)  =  TFTAB 

TMF  (2,1)  =  TMF(LENF(I)-1,I) 

TMF (3,1)  =  TMF (LENF  (I) ,1) 

TMF (4,1)  =  TMF (1,1)  +  TLAGFR 
TMF (5, I)  =  TMF ( i,I)  +  TRUPFR 
TMF (6, I)  =  TMF (4, I)  +  VCMD ( I ) 

T MF(7,I)  =  TMF  (6,1)  +  TRDNFR 

THF ( 1 , I)  =  1.0 

THF  (2,1)  =  1.0 

THF (3,1)  =  0.0 

THF (4, I)  =0.0 

THF  (5,1)  =  1.0 

THF  (6,1)  =  1.0 

THF (7, I)  =  0.0 

LENF (I)  =  7 

NO  NEW  COMMAND  IS  ISSUED  IF  THRESHOLD  IS 

NOT  REACHED 

ELSEIF  (  VCMD (I) . LT . DELON'DTFRU  )  THEN 
TMF  (1, I)  =  TFTAB 
TMF  (2,1)  =  TMF  (LENT  <I)-l,Ii 
TMF  (3,1)  =  TMF (LENF (I) , I) 

THF  (1, I)  =  1.0 
THF (2,1)  =  1.0 
THF  (3, I)  =  0.0 
LENF  (I)  =  3 

ENDIF 


ENDIF 

VALVE  WILL  BE  OPEN  AT  REISSUE  TIME 

ELSEIF  (  TFTAB+ TLAGFR . LT. TMF (LENF ( I ) , I )  )  THEN 

VALVE  WILL  BE  RAMPING  SHUT  AT  REI3SUE  TIME 

IF  (  TFTAB+TLAGFR.GT.TMF (LENF (I) -1, I)  )  THEN 
VLVRES  =  TMF (LENF (I) -1, I)  -  TFTAB 
+  0.5 'TRDNFR 

VCMD (I)  =  VCMD (I)  -  VLVRES 

ISSUE  COMMAND  ONLY  IF  DESIRED  DURATION 
EXCEEDS  RAMP  INTERVAL 

IF  (  VCMD (I) .GE. TRUPFR  )  THEN 

CALL  TABLE (TMF (1,1), THF ( 1 , I) , TFTAB, 
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VCMD (I) 
VCMD ( I ) 

TMF  (1, I) 
TMF (2, I) 
TMF (3, I) 
TMF (4, I) 

TMF (5, I) 
TMF (6, I) 
THF (1, I) 
THF (2, I) 
THF (3, I) 
THF (4, I) 
THF (5, I) 
THF (6, I) 
LENF  (I) 


TMP1, LENF (I) , ITMP ) 

AMIN1  (  DTFRU  ,  VCMD ( I )  ) 

0. 001'ANINT (VCMD (I) / 

0.001) 

TFTAB 

TMF (LENF (I) -1, I) 

TMF (1,1)  +  TLAGFR 
TMF (3, I)  +  TMF (3, I)  - 
TMF (2, I) 

TMF (3, I)  +  VCMD (I) 


TMF (5,1) 
1.0 
1.0 
TMP1 
1.0 
1.0 
0.0 
e 


TRDNFR 


NO  NEW  COMMAND  IS  ISSUED  IF  THRESHOLD  IS 
NOT  REACHED 


ELSEIF  (  VCMD (I) .LT.TRUPFR  )  THEN 


TMF  (1,1) 
TMF  (2,1) 
TMF (3, I) 
THF  (1,1) 
THF (2, I) 
THF (3, I) 
LENF (I) 
ENDIF 


TFTAB 

TMF  (LENF  (I)  -1,  I) 
TMF (LENF  (I) , I) 


VALVE  WILL  BE  WIDE  OPEN  AT  REISSUE  TIME 

ELSEIF (TFTAB+TLAGFR. LE. TMF (LENF ( I ) -1 , I ) )  THEN 

COMPARE  REMAINING  NORMALIZED  IMPULSE  WITH 
REQUESTED  NORMALIZED  IMPULSE  TO  SEE  IF  NEW 
COMMAND  SHOULD  BE  ISSUED 

VLVRES  =  TMF (LENF(I) -1, I)  -  TFTAB 
+  0 . S'TRDNFR 

VCMD (I)  -  VCMD (I)  -  VLVRES 

REVISE  VALVE  SHUT  TIME  IF  ADDITIONAL 
IMPULSE  IS  REQUIRED 


FTN286  X415  OPTIMIZE (3) 
9998  CONTINUE 


IF  (  VCMD (I ) . GT .0.0  )  THEN 


VCMD(T)  = . AMIN1  (  DTFRU  ,  VCMD ( I )  ) 

VCMD ( I >  =  0.001*ANINT(VCMD(I)/ 

0.001) 

TMF (1,1)  =  TFTAB 

TMF  (2,1)  =  TMF (LENF (I) -1, I)  +  VCMD(I) 

TMF  (3, I)  =  TMF (LENF (I) , I)  +  VCMD (I) 

THF  (1,1)  =  1.0 

THF (2,  I)  =1.0 

THF ( 3 , I)  =0.0 

LENF (I)  =  3 


NO  NEW  COMMAND  IS  ISSUED  IF  ADDITIONAL 
IMPULSE  IS  NOT  REQUIRED 


ELSEIF  (  VCMD(I) .LE.0.0  )  THEN 


TMF  (1,  I) 
TMF (2, I) 
TMF (3, I) 
THF (1, I) 
THF (2, I) 
THF (3, I) 
LENF  (I) 
ENDIF 


TFTAB 

TMF  (LENF  (I)  -1,  I) 

TMF (LENF  (I) ,  I) 

1.0 

1  -C 

0.0 

3 


ENDIF 


no noon  non 
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ENDIF 

20  CONTINUE 

ENDIF 

C  DETERMINE  TABLE  LOOKUP  REFERENCE  TIME 

TREF  =  T 

C  CALCULATE  CURRENT  THRUST  LEVELS  FOR  EACH  VALVE 

DO  40  I  =  1  ,  4 

COMPUTE  INSTANTANEOUS  NORMALIZED  THRUST  LEVEL  VIA  TABLE 
LOOKUP  IF  FRACS  CYCLE  IS  SCHEDULED  FOR  THIS  THRUSTER.  ALSO 
EXTRAPOLATE  TIME  OF  NEXT  FRACS  TABLE  LOOKUP  INDEX  TRANSITION. 

IF  (  LENF (I) . GT . 0  )  THEN 

CALL  TABLE (TMF (1,1) , THF (1, I) , TREF, ATHRF (I) , LENF (I) , 

INDX (I) , 

ELSE 

ATHRF (I)  =0.0 
INDX  ( I )  =  0 

ENDIF 


CALCULATE  THE  FORCES  AND  MOMENTS  PRODUCED  BY  THE  FRACS 
THRUSTERS  : 

F (I )  IS  THE  FORCE  ALONG  THE  Ith  AXIS  ,  ADJUSTED 
FOR  MISALIGNMENT  . 

XMOM(I)  IS  THE  EFFECTIVE  MOMENT  ARM. 

THE  MOMENT  GENERATED  IS  (  F  x  XMOM  ) . 

DO  30  J  *  1  ,  3 

F0(.)  =  FRCDIR ( J, I ) 'KN 'KNFAC'TH JET* ATHRF ( I ) 

XMOh ; J )  =  CG ( J)  -  FRCLOC ( J, I ) 

30  CONTINJE 

THRUSTER  MISALIGNMENT  EFFECTS 

F  ( 1  >  *  F0(1)'FRCMA(1,I)  +  F0 ( 2 ) 'FRCMA  !  4 , 1 )  + 

F0 (3) *FRCMA (7, I) 

F (2 )  =  F0 ( 1 ) *FRCMA (2,1)  +  F0 ( 2 ) 'FRCMA (5,1)  + 

F0  (3)  *FRCMA(8,  I) 

F  <  3)  =  F0  ( 1 )  *  FRCMA  (3,1)  +  F0  (2  )  *FRCMA  (6,1)  + 

F0 (3) 'FRCMA (9, I) 

M  ( 1 )  =  F  (2)  'XMOM  (3)  -  F(3)'XMOM(2) 

M  ( 2  )  =  F  (  3 )  'XMOM  ( 1 )  -  F(l)'XMOM(3) 

M  (3  )  =  F  ( 1 )  'XMOM  (2  )  -  F(2)'XMOM(l) 

C  SUM  FORCES  AND  MOMENTS  OF  ALL  THRUSTERS 

FRCX  =  FRCX  +  F (1 ) 

FRCY  =  FRCY  +  F (2) 

FRCZ  =  rRCZ  +  F ( 3) 

MRCX  =  MRCX  +  M (1 ) 

MRCY  =  MRCY  *  M(2) 

MRCZ  =  MRCZ  +  M  <  3 ) 

IF  (  I.EQ.l  .OR.  I.EQ.3  )  THEN 

MRCY  =  MRCY  +  FRCDIR (3,1) 'THJET'KM'KMFAC'DJET' 
ATHRF (I) 

ELSE 

MRCZ  =  MRCZ  -  FRCDIR (2, I) 'THJET'KM'KMFAC'DJET' 
ATHRF (I) 

ENDIF 


40 


FILE: 


MDOTF  =  MDCTF  *  THJET'ATHRF (I) /FISP 

CONTINUE 

ENDIF 

RETURN 

END 


uuv22 . 1 9q/ sut i 1 i ty /uu f vdot . f or 


160 


Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1,  Part  3 


SUBROUTINE  FVDOT  (  W, 

WD,  F,  FD  ) 

c 

c 

SUBROUTINE  NAME  : 

FVDOT 

c 

c 

AUTHOR (S)  : 

W.  E.  EXELY 

c 

c 

FUNCTION  : 

COMPUTE  THE  QUATERNION  DERIVATIVES 

(FD) 

c 

USING  BODY  RATES  (W)  AND  LATENT  INTEGRAL 

c 

DERIVATIVE  (WD)  AND  THE  QUATERNION 

(F) 

c 

c 

CALLED  FROM  : 

FORTRAN  MAIN,  MISSIL 

C 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

c 

INPUTS  : 

W,  WD, F 

c 

c 

OUTPUTS  : 

FD 

c 

c 

UPDATES  : 

D.  SMITH  -  CR  #  59 

c 

r - 

c 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 

C 

DIMENSION  W  (3) ,  F(4),  FD(4) 
C 

W1  =  W  < 1 ) 

W2  -  W  ( 2 ) 

W3  =  W(3) 

W4  =  WD 
FI  =  F  ( 1 ) 

F2  =  F  (2) 

F3  -  F  <  3 ) 

F 4  =  F  (4  ) 


FD  (1 )  - 

W4*F1 

+ 

W1*F4 

-  W2*F3 

+  W3*F2  ) 

*0.5 

FD (2)  = 

W4*F2 

+ 

W1*F3 

+  W2*F4 

-  W3*F1  ) 

*0.5 

FD (3)  = 

W4*F3 

- 

W1*F2 

+  W2*F1 

+  W3*F4  ) 

*0.5 

FD  ( 4 )  » 

W4*F4 

- 

W1*F1 

-  W2*F2 

-  W3*F3  ) 

*0.5 

C 

RETURN 

END 


FILE :  uuv22 . 19g/sutili ty/uugy ro .for 


C 

C- 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  GYRO <T, P, Q, R, CIM, GYSEED, QFRACG, PULSEG) 

SUBROUTINE  NAME  :  GYRO 

AUTHOR  (S)  :  A.  P.  BUKLEY,  M.  K.  DOUBLEDAY 

FUNCTION  :  GYRO  MODEL  COMPUTES  SENSED  DELTA  ANGLE 

COUNTS.  INCLUDES  AXIS  MISALIGNMENT  AND 
NONORTHOGONALITY  ERRORS,  SCALE  FACTOR 
ERRORS, RANDOM  AND  CONSTANT  DRIFT,  AND 
QUANTIZATION. 

CALLED  FROM  :  FORTRAN  MAIN 

SUBROUTINES  CALLED  :  NORM, BRTAVG, RESP2P 
INPU'Y  :  T,  P,  Q,  R,  CIM 

OUTPUTS  :  NONE 


C  BOTH  : 

C 

C  UPDATES 


C 

C 

C 


GYSEED, QFRACG, PULSEG 


T.  THORNTON 
T.  THORNTON 
B.  HILL 
D  SMITH 
B.  HILL 
B.  HILL 


CR  #  004 
CR  *  016 
CR  »  020 
CR  #  021 
CR  *  022 
CR  »  030 
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c 

B. 

HILL 

- 

CR 

* 

038 

c 

D. 

SMITH 

- 

CR 

# 

059 

c 

D. 

SISSOM 

- 

CR 

* 

069 

c 

D. 

SMITH 

CR 

* 

070 

c 

D. 

SMITH 

- 

CR 

* 

075 

c 

D. 

SMITH 

- 

CR 

# 

077 

c 

D. 

SMITH 

- 

CR 

* 

078 

c 

B. 

HILL  / 

- 

CR 

* 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

- 

CR 

* 

083 

c 

R. 

RHYNE 

- 

CR 

# 

084 

c 

R. 

RHYNE 

“ 

CR 

# 

087 

c 

B. 

HILL 

- 

CR 

# 

093 

c 

c 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 


REAL 

CIMO) 

REAL 

DTHET (3) 

REAL 

QFRACG (3) 

REAL 

SFEG (3) 

REAL 

WBI1 (3) 

REAL 

WBOl  (3) 

CIMO (9) 
PULSEG (3) 
SF1G (3) 
WBIQ (3) 
WBI2 (3) 
WB02 (3) 


DCG { 3 ) 
PQRAVG (3) 
SF2G { 3 ) 

WBOO (3) 
WDRG(3) 


INTEGER*4  GYSEED 


C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 


SAVE 


IGYRO 


*  DATA  INITIALIZATION 

SINCLUDE ( ' * /INCLUDE /SSGYRO . DAT ' ) 

$  INCLUDE ( ' A/INCLUDE/SSCON16.DAT' ) 
$INCLUDE ( ' VINCLUDE/SSCON21 . DAT ■ ) 
^INCLUDE <  "VINCLUDE/SSCON53 . DAT • ) 

DATA  IGYRO  /  1  / 

IF  (IGYRO  .EQ.  1)  THEN 

IGYRO  -  0 


C  INITIALIZE  GYRO  PARAMETERS 


IF  (T  .EQ.  0.0)  THEN 

DRSIGG  =  DRSGGI/ (60. 0*SQRT (DTIMU) *DTR) 
CALL  NORM (ALNSGG, ALNMNG, GYSEED, PSIG) 
CALL  NORM (ALNSGG, ALNMNG, GYSEED, THTG) 
CALL  NORM (ALNSGG, ALNMNG, GYSEED, PHIG) 
CALL  NORM ( AORSGG, AORMNG, GYSEED, THXZG) 
CALL  NORM (AORSGG, AORMNG, GYSEED, THXYG) 
CALL  NORM (AORSGG, AORMNG, GYSEED, THYZG) 
CALL  NORM (AORSGG, AORMNG, GYSEED, THYXG) 
CALL  NORM (AORSGG, AORMNG, GYSEED, THZYG) 
CALL  NORM (AORSGG, AORMNG, GYSEED, THZXG) 
CALL  NORM (SF1SGG, SF1MNG, GYSEED, SF1G (1 ) ) 
CALL  NORM (SF1SGG, SF1MNG, GYSEED, SF1G (2) ) 
CALL  NORM (SF1SGG, SF1MNG, GYSEED, SF1G (3) ) 
CALL  NORM (SF2SGG, SF2MNG, GYSEED, SF2G ( 1 ) ) 
CALL  NORM (SF2SGG, SF2MNG, GYSEED, SF2G (2) ) 
CALL  NORM (SF2SGG, SF2MNG, GYSEED, SF2G (3) ) 
CALL  NORM (DCSIGG, DCMENG, GYSEED, DCG(l) ) 
CALL  NORM (DCSIGG, DCMENG, GYSEED, DCG (2) ) 
CALL  NORM(DCSIGG, DCMENG, GYSEED, DCG!3) ) 
DO  10  I  =  1,3 
WBI2 (I)  =0.0 
WBI 1 (I)  =  0.0 
WB02 ( I )  =  0.0 
WBOl (I)  =0.0 
10  CONTINUE 

ENDIF 


C  COMPUTE  SECOND  ORDER  RESPONSE  DIFFERENCE  EQUATION  COEFFICIENTS 

IF  (  IGRTYP . EQ . 2  )  THEN 

CALL  RESP2R  (  DTIMU, WGYR, ZGYR, CW3I 2, CWBI 1 , CWBI 0 , CWB02, CWBOl , 
CWBOO  ) 


ENDIF 
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ENDIF 

C  COMPUTE  DELTA  TIME  SINCE  LAST  PASS  THROUGH  GYRO 

DTDEL  =  T  -  TOGYRO 
TOGYRO  -  T 

C  DETERMINE  AVERAGE  BODY  RATE  OVER  LAST  INTERVAL 

IF  (  DTDEL. NE. 0.0  )  THEN 

CALL  BRTAVG  (  CIM  ,  CIMO  ,  DTDEL  ,  PQRAVG  ) 

ELSE 

PQRAVG  (1)  =  P 
PQRAVG (2)  =  Q 
PQRAVG (3)  -  R 
ENDIF 

C  SAVE  INERTIAL-TO-MISSILE  ROTATION  MATRIX  FOR  NEXT  PASS 

DO  20  I  -  1  ,  9 

CIMO  ( I )  ■=  CIM  (I) 

20  CONTINUE 

C  GYRO  AXIS  MISALIGNMENT  EFFECTS 

PM  =  PQRAVG (1 )  +  PQRAVG (2) *PSIG  -  PQRAVG (3) *THTG 

QM  =  PQRAVG (2)  -  PQRAVG ( 1 ) *PSIG  +  PQRAVG (3) *PHIG 

RM  -  PQRAVG ( 3 )  +  PQRAVG <1 ) *THTG  -  PQRAVG (2) *PHIG 

C  GYRO  AXIS  NONORTHOGONALITY  EFFECTS 


PN 

«  PM  + 

QM*THXZG 

-  RM'THXYG 

QN 

-  QM  - 

PM'THYZG 

+  RM*THYXG 

RN 

=  RM  + 

PM'THZYG 

-  QM'THZXG 

C  ADD  LINEAR  AND  QUADRATIC  SCALE  FACTOR  ERRORS 

SFEG(l)  =  PN  +  SF1G(1)*PN  +  SF2G (1) *PN**2 

SFEG (2)  -  QN  +  SF1G(2)*QN  +  SF2G (2) *QN**2 

SFEG ( 3 )  *  RN  +  SF1G(3)*RN  +  SF2G (3) *RN**2 

C  FOR  EACH  AXIS  . . . 

DO  30  I  -  1.3 

C  MAKE  A  GAUSSIAN  DRAW  FOR  RANDOM  DRIFT  AND  ADD  TO  CONSTANT 

C  DRIFT 

IF  (  DRSIGG.GT. 0 . 0  )  THEN 

CALL  NORM (DRSIGG, DRMENG, GYSEED,  DRG! 

ENDIF 

WDRG(I)  =  DRG  +  DCG(I) 

C  COMPUTE  INPUT  TO  GYRO  RESPONSE  MODEL 

WBIO(I)  ■=  SFEG  ( I )  +  WDRG(I) 

C  SECOND  ORDER  RESPONSE  MODEL 

IF  (  IGRTYP.EQ.2  )  THEN 

WBOO(I)  *  (  CWBI0*WBI0 (I)  +  CWBI 1 *WBT 1  I T ) 

+  CWBI2*WBI2 (I)  -  CWBOl *WB01 ( I ) 

-  CW302*WB02 (I)  l/CWBOO 
WBI2 (I)  =  WBI 1  (I) 

WBI1 (I)  -  WBI 0  (I) 

WB02 (I)  -  WBOl  (I) 

WBOl (I)  «  WBOO (I) 

ENDIF 

C  INSTANTANEOUS  RESPONSE  MODEL 

IF  (  IGRTYP.EQ.O  )  THEN 
WBOO (I)  =  WBI 0 ( I ) 

ENDIF 

C  COMPUTE  DELTA  THETA 

DTHET(I)  -  DTDEL  *  WBOO  (I) 


IF  (  SPPG.GT.0.0  )  THEN 
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C  UNQUANTIZED  OUTPUT  IN  COUNTS 

QFRACG(I)  =  QFRACC  _  /  -  r  uLSEG  ( I ,  -  OTHET  Ui  /SEPC 

C  QUANTIZED  OUTPUT  IN  COUNTS 

PULSEG (I )  »  AINT (QFRACG (I) ) 

ELSE 

PULSEG (I)  =  DTHET(I) 

ENDIF 

30  CONTINUE 

RETURN 

END 


FILE:  uuv22 . 19g/sutility/uuinteg. for 


C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE 

INTEG  (  X 

,  XDOT  ,  T  ,  I  ) 

SUBROUTINE 

NAME  : 

INTEG 

AUTHOR (S)  : 

D.  F.  SMITH 

FUNCTION  : 

Perform  simple  trapezoidal  integration  of 
XDOT  to  yield  X.  DTD  is  the  time  since 
the  last  integration  and  I  is  the  array 
index  where  X  is  stored 

CALLED  FROM 

: 

FORTRAN  MAIN 

SUBROUTINES 

CALLED  : 

NONE 

INPUTS  : 

XDOT, T, I 

OUTPUTS  : 

X 

UPDATES  : 

D.  SISSOM  -  CR  #  58 

D.  SMITH  -  CR  #  59 

COMMON /STORAG/ 
REAL  XINT ( 50 ) , 

REAL  DT, 

REAL  XDOT, 


XINT, 

TINT (50) , 
DTMP , 

T 


TINT,  XDOTL 

XDOTL ( 50 ) 

X 


DT 


T  -  TINT ( I ) 


XINT ( I )  =  XINT ( I )  +  0. 5*DT* (XDOT+XDOTL ( I ) ) 

X  »  XINT (I ) 


TINT ( I )  =  T 

XDOTL (I)  =  XDOT 


C  TEMPORARY  CODE  TO  NORMALIZE  QUATERNION  AFTER  4TH  COMPONENT  IS  REVISED 


IF  (  I.EQ.18  )  THEN 

DTMP  =  SQRT  (  XINT (15) * *2  h 
XINT (10) *  *  2 


END 


XINT (15) 
XINT  (16) 
XINT  (17) 
XINT (18) 
IF 


XINT  (16)  **2  +  XINT!17)**2  + 

) 


XINT (15) 
XINT  (16) 
XINT (17) 
XINT (18) 


DTMP 

DTMP 

DTMP 

DTMP 


RETURN 

END 


FILE:  uuv22 .19g/sutility/uuintegi.for 


c - 

SUBROUTINE  INTEGI  (  X  ,  XDOT  ,  T  ,  I  ) 
C - 


onoonoooooonnoonooo 


164  Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1,  Part  3 
c 


c 

c 

SUBROUTINE  NAME  : 

INTEGI 

c 

c 

AUTHOR (S)  : 

D.  F.  SMITH 

c 

c 

c 

FUNCTION  : 

Initialize  integral  of  X  which  is  stored 
in  position  I  of  the  integral  array 

c 

c 

CALLED  FROM  : 

MAIN 

c 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

c 

INPUTS  : 

X,  XDOT, T, I 

c 

c 

OUTPUTS  : 

NONE 

c 

c 

UPDATES  : 

D.  SISSOM  -  CR  i  58 

D.  SMITH  -  CR  #  59 

c - 

COMMON/STORAG/ 
REAL  XINT (50 ) , 
REAL  X, 

XINT  (I)  =  X 

XDOTL(I)  -  XDOT 
TINT  (I)  =  T 

RETURN 

END 


FILE:  uuv22 . 19g/sutility/uum3x3i -for 


C 


SUBROUTINE  M3X3I  (  A  ,  B  ) 


XINT,  TINT,  XDOTL 

TINT ( 50 ) ,  XDOTL (50' 

T,  XDOT 


SUBROUTINE  NAME  : 
AUTHOR (S)  : 
FUNCTION  : 

CALLED  FROM  : 
SUBROUTINES  CALLED 
INPUTS  : 

OUTPUTS  : 

UPDATES  : 


M3X3I 

D.  F.  SMITH 

Compute  the  inverse  of  a  3  by  3  matrix  . 

UTILITY  ROUTINE 

NONE 

A 

B 

NONE 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 


REAL  A<3,3),  L'3,3) 


DET 


-  AU,1)*A(2,2)*A<3,3) 
+  A(1,2>»A<2,3)*A(3,1) 
+  All,  3)  *A<2, 1)  *A<3,  2) 


-  A(1,1)*A(2,3)  *A  ( 3 , 2 ) 

-  A(l,  2)  *A<2, 1)  *A(3,  3) 

-  A  (1, 3)  *A<2, 2)  *A  (3,  1) 


IF  (  DET. NE. 0.0  )  THEN 


B  (1, 1) 
B<2,  1) 
B(3,  1) 
B  (1, 2) 
B<2, 2) 
B  ( 3 , 2 ) 
B  (I,  3) 
B  ( 2 ,  3) 
B  { 3 , 3) 
ELSE 

B (1 , 1) 
B(2, 1) 
B(3,  1) 


=  ( 


=  ( 
=  ( 
-  ( 
=  ( 
=  ( 
=  ( 


A<2,  2)  *A(3,  3) 
A  (2 ,  3)  *A  ( 3 ,  1) 
A  (2,  1)  *A  !  3,  2) 
A  (1, 3)  *  A  ( 3 ,  2) 
A  (1, 1)  *A  (3, 3) 
A(l,  2)  *A(3,  1) 
A(l,  2)  *A  ( 2 ,  3) 
A(l, 3) *A(2, 1) 
All,  1)  *A(2,  2) 


0.0 

0.0 

0.0 


A 

( 2 , 

3) 

*A 

(3, 

2) 

) 

/ 

DET 

A 

<2, 

, '  \ 

*  A 

(3, 

3) 

) 

/ 

DET 

A 

(2, 

,  z) 

*A 

<3, 

1> 

) 

/ 

DET 

A 

(1, 

2) 

*  A 

(3, 

3) 

) 

/ 

DET 

A 

(1, 

3) 

*A 

(3, 

1) 

) 

/ 

DET 

A 

(1, 

1) 

#A 

(3, 

2) 

) 

/ 

DET 

A 

(1, 

.  3) 

*A 

(2, 

2) 

) 

/ 

DET 

A 

(1. 

.  1) 

•A 

<2, 

3) 

) 

/ 

DET 

A 

(1, 

2) 

*A 

(2, 

1) 

> 

/ 

DET 
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B  ( 1 , 2 )  =  0.0 
B(2,2)  =  0.0 
B(3,2)  =  0.0 
B  ( 1 , 3 )  =  0.0 
B(2,3)  =  0.0 
B (3, 3)  =  0.0 
END  IF 

RETURN 

END 


FILE:  uuv22.19g/sutility/uumcguid. for 


C 


C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE 

MCGUID (T, TI2M,  VG, URREL, MASS, IDIST, MIDBRN, MAGR, MAGV, SP, 
SQ,  SR, PITER, YAWER, FLIP, IVCS, ICMD, IDMEAS, IDPASS, 
IDROP, IMCEND, IBURND, IBURNM, VGM, ADISTT, ROLLER, 
TMGUID) 

SUBROUTINE 

NAME  :  MCGUID 

AUTHOR  : 

R.  RHYNE 

FUNCTION  : 

DEFINES  ROLL  ERROR,  SEQUENCES  MIDCOURSE 

EVENTS,  AND 

ENABLES  MIDCOURSE  DIVERTS 

CALLED  FROM 

:  FORTRAN  MAIN 

SUBROUTINES 

CALLED  :  NONE 

INPUTS  : 

T, TI2M, VG, URREL, MASS, IDIST, MIDBRN, MAGR, 

MAGV, SP, SO, SR, PITER, YAWER, FLIP, ICMD 

OUTPUTS  : 

IDMEAS, IDPASS, IMCEND, IBURND, IBURNM, VGM, 

ADISTT, ROLLER, TMGUID 

BOTH  : 

IDROP 

UPDATES  : 

B.  HILL  / 

-  CR  t  081 

R.  RHYNE 

R.  RHYNE 

-  CR  *  083 

R.  RHYNE 

-  CR  #  084 

R.  RHYNE 

-  CR  #  083 

R.  RHYNE 

-  CR  *  090 

B.  HILL 

-  CR  #  093 

IMPLICIT  REAL  (A-H) 
IMPLICIT  REAL  (O-Z) 


REAL  TI2M ( 9) 
REAL  MASS 
REAL  VGM ( 3 ) 
REAL  VGP (3) 
REAL  RATE  (6) 
INTEGER 
INTEGER 


,  VG ( 3 ) 

,  MAGR 

,  ADISTT  (4,3) 
,  VGPM ( 3 ) 

,  TRGSIG(4) 
ISEQM) 
BCKGRD 


,  URREL (3) 

,  MAGV 
,  OMEGAO ( 3 ) 

,  ACQRNG ( 4 , 4 ) 

FLIP  ,  SEKTYP 


C 


LOCAL  DATA  USED 


FOR  CONSTANTS  AND  INITIALIZATION  FLAG 


SAVE 


IMGUID 


*  DATA  INITIALIZATION 
SINCLUDE  ( '  VINCLUDE/SSMCGUID.DAT’  ) 
SINCLUDE ( 1 A/INCLUDE/SSCON46.DAT' ) 
SINCLUDE  ( '  A/INCLUDE/SSCON48.DAT’  ) 
SINCLUDE  ( •  “/INCLUDE/ SSCON 50  .  DAT  1  ) 
SINCLUDE ( 1 “/INCLUDE/ SSCON 55 . DAT’ ) 
SINCLUDE ( ’ * /INCLUDE /SSCON 60 . DAT  1 ) 
SINCLUDE ( ’ “ / INCLUDE /SSCON 61 . DAT • ) 
SINCLUDE (  ”'/ INCLUDE/ SSCON 62 .DAT’ ) 
SINCLUDE ( 1  “ / INCLUDE /SSCONO 1 . DAT  1 ) 
SINCLUDE ( ’ “ / INCLUDE/ SSCONO A . DAT ’ ) 
SINCLUDE ( ’ “/  INCLUDE/ SSCONO 5 .DAT’ ) 
SINCLUDE  (  ’“/ INCLUDE /SSCONO 9 .  DAT  ’  ) 
SINCLUDE  (  ’“/INCLUDE/SSCON12.DAT’  ) 
SINCLUDE ( ’“/ INCLUDE /SSCON1 3 . DAT ’ ) 
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S  INCLUDE  ( *  '“/INCLUDE/SSC0N17  .  DAT  1 ) 

DATA  IMGUID  /  1  / 

IF  (  IMGUID  .EQ.  1  )  THEN 
IMGUID  -  0 

IF  (  SEKTYP . EQ. 2  )  THEN 
TSIG  »  TRGSIG (ITRGSG) 

TSGACQ  =  TSIG 

RAQREF  =  ACQRNG (BCKGRD, ITRGSG) 

RNGAQ  =  SQRT ( (TSGACQ/ TSIG) ’  ( 6 . 0/SNRACQ) * 

(SQRT (1. /RATE (1) ) ) ) *RAQREF 

ELSE  IF  (  SEKTYP. EQ. 3  )  THEN 
RNGAQ  =  ACQR3 
ELSE 

RNGAQ  =  RNGACQ 
ENDIF 
ENDIF 

C  GET  VG  IN  BODY  FRAME 

VGM(l)  *  TI2M (1 ) *VG (1 )  +  TI2M <4 ) * VG (2 )  +  TI2M ( 7 ) * VG ( 3 ) 

VGM  (2)  -  TI2M(2)*VG(1)  +  TI2M (5 ) 'VG (2)  +  TI2M  (8) 'VG  (3) 

VGM ( 3 )  »  TI2M (3) *VG (1)  +  TI2M ( 6) *VG (2 )  +  TI2M ( 9) 'VG ( 3 ) 

C  CALCULATE  ROLL  ERROR  IF  KV  REORIENTATION  AND  UPLINK  HAVE  OCCURRED 

IF  (  FLIP . EQ. 0  .AND.  T.GE.TUPLK1  .AND.  IMCEND . EQ. 0  )  THEN 

VGDLOS  -  URREL(l) *VG<1)  +  URREL (2) 'VG (2)  +  URREL (3) 'VG (3) 

C  DETERMINE  PERPENDICULAR  COMPONENT  OF  VG 

VGP(l)  =  VG ( 1 )  -  VGDLOS* URREL ( 1 ) 

VGP (2)  =  VG (2)  -  VGDLOS 'URREL (2) 

VGP<3)  =  VG(3)  -  VGDL0S'URREL(3) 

C  GET  VGP  IN  BODY  FRAME 

VGPM(l)  -  TI2M(1) 'VGP (1)  +  TI2M ( 4 ) *VGP ( 2 )  +  TI2M (7) 'VGP (3) 

VGPM (2)  =  TI2M(2) 'VGP (1)  +  TI2M ( 5 ) 'VGP ( 2 )  +  TI2M (8) *VGP (3) 

VGPMO)  =  TI2M  (3)  *VGP  (1)  +  TI2M  1 6)  '  VGP  (2)  +  TI2M  (  9) 'VGP  ( 3 ) 

IF  (  VGPMO)  .NE.0.0  )  THEN 

RERR  «  -ATAN2  (VGPM(2)  ,  VGPMO)  ) 

ELSE 

PI02  -  PI/2. 

RERR  -  -SIGN (PI02, X) 

ENDIF 

C  ESTIMATE  REQUIRED  DIVERT  DURATION 

ACM  -  FLATM/MASS 
TBURNY  =  ABS (VGPM (2) /ACM) 

TBURNZ  *  ABS (VGPM (3) /ACM) 

TBURN  =  AMAX1 (TBURNY,  TBURNZ) 

BYPASS  MAJOR  ROLL  CORRECTION  IF  BURN  TIME  ALONG  EITHER 
AXIS  IS  BELOW  VCS  BURN  THRESHOLD 

IF  (  TBURN. LT.TCMINV  .AND.  ICMD.EQ.O  )  THEN 
ROLLER  -  0. 

IVCS  =■  0 

ELSE  IF  (  ABS (TBURNY) .LT.TCMINV  .AND.  ICMD.EQ.O  )  THEN 
ROLLER  =  0. 

IF  (  VGPMO)  .GT.  0.  )  THEN 
IVCS  =  4 
ELSE 

IVCS  =  2 
ENDIF 

ELSE  IF  (  ABS (TBURNZ) .LT.TCMINV  .AND.  ICMD.EQ.O  )  THEN 
dot  t  rq  -  0  . 

IF  (  VGPM ( 2 >  .GT.  0.  )  THEN 
IVCS  =  3 
ELSE 

IVCS  =  1 
ENDIF 

C  DEFINE  ROLL  ERROR  TO  ALIGN  VGPM  WITH  NEAREST  VCS  THRUSTER 


ELSE  IF  (  ICMD  . EQ .  0  )  THEN 


n  o 
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IF  (  ABS(RERR)  . LE .  PI/4.  )  THEN 
ROLLER  =  RERR 
IVCS  =  4 

ELSE  IF  (  RERR  . LE .  -3. 'PI/4.  )  THEN 
ROLLER  =  PI  +  RERR 
IVCS  =  2 

ELSE  IF  (  RERR  .GE.  3. 'PI/4.  )  THEN 
ROLLER  =  RERR  -  PI 
IVCS  =  2 

ELSE  IF  (  RERR  .  LT  .  3  .  *PI  /  4  .  ..  RERR .  GT .  PI /4  .  )  THEN 
ROLLER  =  RERR  -  PI/2. 

IVCS  =  1 
ELSE 

ROLLER  =  RERR  +  PI/2. 

IVCS  =  3 
ENDIF 

IF  ATTITUDE  CORRECTION  IN  PROGRESS,  USE  SAME 
ROLL  ERROR  CALCULATION 

ELSE 

IF  (  IVCS  .EQ.  1  )  THEN 
ROLLER  »  RERR  -  PI/2. 

ELSE  IF  (  IVCS  .EQ.  2  )  THEN 
IF  (  RERR  .LT.  0.  )  THEN 
ROLLER  =  PI  +  RERR 
ELSE 

ROLLER  =  RERR  -  PI 
ENDIF 

ELSE  IF  (  IVCS  .EQ.  3  )  THEN 
ROLLER  =  RERR  +  PI/2. 

ELSE 

ROLLER  =  RERR 
ENDIF 
ENDIF 

ELSE 

C  ZERO  ROLL  ERROR  IF  PITCHOVER  AND  FIRST  UPLINK  HAVE  NOT  OCCURRED 

ROLLER  =  0. 

ENDIF 

IF  (  IDMEAS.EQ.O  .AND.  ICMD.EQ.O  .AND.  ABS (PITER) . LE . CATHL 
.AND.  ABS (YAWER) . LE. CAPS L  .AND.  (IGIT.EQ.O  .OR. 

(IGIT.EQ.l  .AND.  T.GE.TDROP))  )  THEN 

C  ENTER  DISTURBANCE  MEASUREMENT  MODE 

CALL  OUTMES(0801,T,0.0) 

IDMEAS  =  2 
ENDIF 

IF  (  IDMEAS. EQ. 2  .AND.  ABS (SP) . LE .CRPHL  .AND.  ABS (SQ) . LE . CRTH 
•AND.  ABS (SR) .LE.CRPS  .AND.  ICMD.EQ.O  )  THEN 

IF  (  IDPASS  .EQ.  0  )  THEN 

C  DEFINE  VCS  DISTURBANCE  SEQUENCE 

IF  (  ABS (VGM ( 2 ) )  -GE.  ABS(VGM(3))  )  THEN 
INDEXY  =  1 
INDEXZ  =  3 
ELSE 

INDEXY  =  3 
INDEXZ  «  1 
ENDIF 

IF  (  VGM ( 2 )  .GE.  0.  )  THEN 
ISEQ (INDEXY)  =  3 
ISEQ (INDEXY+1)  =  1 
ELSE 

ISEQ (INDEXY)  =  1 
ISEQ (INDEXY ♦ 1 )  =  3 
ENDIF 

IF  (  VGM ( 3 )  -GE.  0.  )  THEN 
I SEQ ( INDEXZ )  =  4 
ISEQ (INDEXZ+1)  =  2 
ELSE 

I SEQ  1  INDEXZ )  =  2 
ISEQ ( INDEXZ+1 )  = 


4 


nnnon  on  o  o  nnnn 
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ENDIF 
IDPASS  =  I 
ENDIF 

IF  (  IBURND  .EQ.  0  )  THEN 

DROP  BOOST  ADAPTER  AND  NOSE  FAIRING  PRIOR  TO  FIRST 
DISTURBANCE  BURN  -  IF  EVENT  DRIVEN  LOGIC,  SCHEDULE 
SEPARATION  HERE  -  OTHERWISE,  SEPARATION  WILL  OCCUR 
AT  T=TDROP  IN  MAIN  ROUTINE 

IF  (  I DROP . EQ. 0  .AND.  IGIT.EQ.O  )  THEN 
IDROP  ■=  1 
ELSE 

DEFINE  Ith  DISTURBANCE  BURN 

IBURND  =  1 
IBURNM  =  0 

TVCOMP  =  T  +  TLAGV  +  TBURND  +  TRDNV  +  TIWAIT 
IVCS  -  ISEQ (IDPASS) 

OMEGAO (1 )  =  SP 
OMEGAO (2)  =  SQ 
OMEGAO (3)  =  SR 
ENDIF 

ELSE  IF  (  T  .GT.  TVCOMP  )  THEN 

COMPUTE  ANGULAR  ACCEL  INDUCED  BY  PREVIOUS  DISTURBANCE  BURN 
IBURND  =  0 

ADISTT (ISEQ (IDPASS) , i)  =  (SP  -  OMEGAO (1) ) /TBURND 
ADISTT (ISEQ (IDPASS) , 2)  -  (SQ  -  OMEGAO (2) ) /TBURND 
ADISTT (ISEQ (IDPASS) , 3)  =  (SR  -  OMEGAO ( 3 )) /TBURND 
IDPASS  -  IDPASS  +  1 
TVCOMP  -  1000. 

IF  (  IDPASS  .GT.  4  )  THEN 
IDMEAS  =■  1 

CALL  OUTMES (0802, T, 0.0) 

ENDIF 
ENDIF 
ENDIF 

ENABLE  SEEKER  AFTER  PITCHOVER  AND  DISTURBANCE 
MEASUREMENT  COMPLETED 

IF  (  ABS (PITER) .LE.CATH  .AND.  ABS (YAWER) . LE . CAPS 
.AND.  ABS (SQ) .LE.CRTH  .AND.  ABS (SR) . LE . CRPS 
-AND.  FLIP . EQ . 1  .AND.  IDMEAS. EQ.l  )  THEN 

ENABLE  SEEKER  (TYPES  0,1, S2)  IF  EVENT  DRIVEN  LOGIC  - 
OTHERWISE  WILL  BE  ENABLED  BY  MAIN  ROUTINE  AT  SECOND 
STAGE  SEPARATION  -  SEEKER  TYPE  3  HANDLED  BELOW  - 
TYPE  3  ENABLED  BY  MAIN  ROUTINE  AT  T=TSK30N  IF  EVENT 
LOGIC  NOT  USED 

FLIP  «=  0 

CALL  OUTMES (0803, T, 0.0) 

ENDIF 

C  DEFINE  THREE  MIDCOURSE  DIVERTS 

IF  (  ABS (ROLLER) .LE. CAP H  .AND.  ABS ( SP ) . LE . CRPH 
■AND.  ICMD.EQ.O  .AND.  T.GT.TUPLK1  )  THEN 
DELMID  =  (  MAGR  -  RNGAQ  ) /MAGV 
IF  (  ICMD.EQ.O  .AND.  MIDBRN . EQ . 0  )  THEN 
IBURNM  =  0 
IMIDB2  -  1 

ELSE  IF  (  IDIST.EQ.O  .AND.  MIDBRN. EQ.l  .AND.  IMIDB2.EQ.1  )  THEN 
TMIDB2  »  T  +  0.5*DELMID 
IMIDB2  -  0 

ELSE  IF  (  T.GE.TMIDB2  .AND.  MIDBRN. EQ.l  )  THEN 
IBURNM  =  0 

ELSE  IF  (  IDIST.EQ.O  .AND.  MIDBRN. EQ. 2  )  THEN 
TMAX  =  TBURN  +  TBWAIT 
IF  (  DELMID  .LE.  TMAX*DTMC'J  )  THEN 
IBURNM  -  0 
ROLLER  =  0. 

IMCEND  =■  1 
ENDIF 
ENDIF 
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ENDIF 

C  COMPUTE  TIME  OF  NEXT  CALL 

TMGUID  =  T  +  DTMCU  -  EPSL 

RETURN 

END 

FILE :  uuv22 . 19g/sutility/uumisslr . for 


C- 

C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  MISSLR (T,  QUAT, CIM, P, Q, R, IXX, IYY, IZZ, MASS, FXA, FXT, 
FRCX, FXACS,  FXVCS , 

MX A, MXT, MRCX, MXACS, MXVCS, 

MYA, MYT, MRCY, MYACS, MYVCS, MZA, MZT, MRCZ, MZACS, 
MZVCS,  X,  Y,  Z,  PD,  QD,  RD, 

MX ,  MY ,  MZ , 

QUATD) 


SUBROUTINE  NAME  :  MISSILR 

AUTHOR (S)  :  D.  C.  FOREMAN,  A.  P.  BUKLEY 

FUNCTION  :  COMPUTES  THE  ROTATIONAL  MISSILE  ACCELERATIONS 

CALLED  FROM  :  FORTRAN  MAIN 

SUBROUTINES  CALLED  :  FVDOT 

INPUTS  :  T, QUAT, CIM, P,Q,R, IXX, IYY, IZZ, MASS, FXA, 

FXT, FRCX, FXACS, FXVCS, 

MXA.MXT, 

MRCX , MXACS , MXVCS , MYA , MYT , MRCY , MYACS , MYVCS , 
VTA,  MZT, MRCZ, MZACS, MZVCS, X, Y, Z, 

OUTPUTS  :  PD,  QD,  RD, MX, MY, MZ, 

QUATD 


c 

UPDATES  : 

D. 

SISSOM 

- 

CR 

* 

Oil 

c 

T. 

THORNTON 

- 

CR 

# 

012 

c 

T. 

THORNTON 

- 

CR 

# 

018 

c 

B. 

HILL 

- 

CR 

# 

030 

c 

T. 

THORNTON 

- 

CR 

* 

031 

c 

T. 

THORNTON 

- 

CR 

* 

033 

c 

T. 

THORNTON 

- 

CR 

* 

035 

c 

T. 

THORNTON 

- 

CR 

# 

037 

c 

T. 

THORNTON 

- 

CR 

# 

04  9 

c 

T. 

THORNTON 

- 

CR 

* 

050 

c 

D. 

SMITH 

- 

CR 

* 

059 

c 

D. 

SMITH 

- 

CR 

* 

060 

c 

B. 

HILL 

- 

CR 

* 

062 

c 

D. 

SMITH 

- 

CR 

# 

076 

c 

R. 

RHYNE 

- 

CR 

# 

079 

c 

B. 

HILL  / 

- 

CR 

* 

081 

c 

R. 

RHYNE 

c 

R. 

RHYNE 

- 

CR 

# 

087 

c 

B. 

HILL 

- 

CR 

# 

093 

C 


C 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 


REAL 

CIM  ( 9) 

,  CMI  ( 9) 

,  GB(3) 

PF.AL 

GR  ( 3 ) 

,  IXX 

,  IYY 

REAL 

IZZ 

,  MASS 

,  MGR 

REAL 

MRCX 

,  MRCY 

,  MRCZ 

REAL 

MX 

,  MXA 

,  MXACS 

REAL 

MXT 

,  MXVCS 

,  MXYZ 

REAL 

MXYZDD 

,  MY 

,  MYA 

REAL 

MYACS 

,  MYT 

,  MYVCS 

REAL 

MZ 

,  MZA 

,  MZACS 

REAL 

MZT 

,  MZVCS 

,  PQR ( 3 ) 

REAL 

QUAT  {4) 

,  QUATD (41 

,  UXYZ<3) 

REAL 

UXYZDD (3) 

,  XYZLCH ( 3 ) 
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C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IMISL 

*  DATA  INITIALIZATION 

SINCLUDE  (  ■•'/INCLUDE/SSMISSIL.DAT1  ) 

SINCLUDE  ( '  '‘/INCLUDE/SSCON28  .  DAT'  ) 

SINCLUDE ( *  * / INCLUDE /SSC0N3 9 . DAT • ) 

SINCLUDE ( •  "V  INCLUDE /SSCON 63 . DAT ' ) 

DATA  IMISL  /  1  / 

DATA  NCLEAR  /  0  / 

IF  (IMISL  .EQ.  1)  THEN 

IMISL  =  0 

C  COMPUTE  MISSILE  LAUNCH  POSITION  IN  INERTIAL  FRAME 

CMI  (1)  «  CIM(l) 

CMI (2)  -  CIM ( 4 ) 

CMI (3)  -  CIM (7) 

CMI (4)  =  CIM (2) 

CMI  (5)  =  CIM  (5) 

CMI (6)  =  CIM ( 8 ) 

CMI  (7)  =  CIM  (3) 

CMI  (8)  =  CIM ( 6) 

CMI (9)  =  CIM ( 9) 

IF  (T  .EQ.  0.0)  THEN 

XYZLCH (1)  =  XLNCH*CMI(1)  +  RADE 
XYZLCH (2)  =  XLNCH*CMI (2) 

XYZLCH ( 3 )  =  XLNCH*CMI (3) 

ENDIF 

C  DETERMINE  initial  GRAVITY  VECTOR,  just  for  seeing  if  we  are  still 

*  on  the  ground  later 

MXYZ  -  SQRT  (  X**2  +  Y**2  +  Z**2  ) 

MGR  =  GMU  /  MXYZ* *2 

IF  (  MXYZ. GT. 0.0  )  THEN 

*  FTN286  X41 5  OPTIMIZE(3) 

99999  CONTINUE 

UXYZ(l)  =  X  /  MXYZ 
UXYZ  (2)  =  Y  /  MXYZ 
UXYZ  < 3 )  =  Z  /  MXYZ 
ELSE 

UXYZ ( 1 )  =0.0 
UXYZ ( 2 )  =  0.0 
UXYZ  (3)  =  0.0 
ENDIF 

C  CALCULATE  GRAVITY  VECTOR  IN  INERTIAL  AND  BODY  FRAMES 

GR ( 1 )  »  -  MGR*UXYZ (1) 

GR (2)  «  -  MGR*UXYZ (2) 

GR ( 3)  =  -  MGR*UXYZ (3) 

GB  ( 1 )  =  CIM  (1)  *GR  (1)  +  CIM  ( 4  )  *GR  ( 2  )  +  CIM(7)*GR{3) 

GB  (2)  =  CIM  (2 )  *GR  ( 1 )  *  CIM(5)*GR(2)  +  CIM(8)*GR(3) 

GB  (3)  =  CIM  (3 )  *GR  ( 1 )  +  CIM(6)*GR(2)  +  CIM(9)*GR(3) 

ENDIF 

C  CALCULATE  TOTAL  X  FORCE,  just  to  see  if  still  on  ground  later 

FX  =  FXT  +  FXA  +  FRCX  +  FXACS  +  FXVCS 

C  CALCULATE  TOTAL  MOMENTS 

MX  =  MXA  +  MXT  +  MRCX  +  MXACS  +  MXVCS 

MY  =  MYA  +  MYT  +  MRCY  +  MYACS  +  MYVCS 

MZ  =  MZA  +  MZT  +  MRCZ  +  MZACS  +  MZVCS 

C  MISSILE  CLEARED  THE  LAUNCHER 

IF  (  NCLEAR. EQ.l  )  THEN 

PD  =  MX/IXX  +  Q*R* ( (IYY-IZZ) /IXX) 

QD  =  MY/ I YY  +  R*P* ( (IZZ-IXX) /IYY) 

RD  =  MZ/IZZ  +  P*Q* ( (IXX-IYY) /IZZ) 
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C  MISSILE  STILL  ON  GROUND 


ELSE  IF  (  FX/MASS . LE . ABS (GB ( 1 ) )  )  THEN 


PD 

=  0.0 

QD 

=  0.0 

RD 

=  0.0 

MISSILE 

OFF  GROUND  BUT  NOT  CLEAR  OF  THE  LAUNCHER 

ELSE  IF 

(  X . LE . XYZLCH ( 1 ) 

•AND.  Y. LE. XYZLCH (2)  .AND 

Z.LE.XYZLCH (3) 

)  THEN 

PD 

=  MX /I XX  +  Q*R* 

( (IYY-IZZ) /IXX) 

QD 

=  0.0 

RD 

=  0.0 

MISSILE 

JUST  NOW  CLEARING 

LAUNCHER 

ELSE 

NCLEAR  =  1 

CALL 

OUTMES (0901, T, 0.0) 

PD 

=  MX/IXX  +  Q*R* 

( (IYY-IZZ) /IXX) 

QD 

=  MY/IYY  +  R*P* 

( (IZZ-IXX) /IYY) 

RD 

=  MZ/IZZ  +  P*Q* 

( (IXX-IYY) /IZZ) 

ENDTF 


C  COMPUTE  QUATERNION  DERIVATIVES 

PQR(l)  =  P 
PQR  (2)  =  Q 
PQR(3)  =  R 

TMP1  =0.0 

CALL  FVDOT (PQR, TMP1 , QUAT, QUATD) 

RETURN 

END 


FILE :  uuv22 . 1 9g/sut ility /uummk . for 


C- 

c- 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


c 

c 


c 


SUBROUTINE  MMK (A, NA, B, NB, C, NC, RM) 


SUBROUTINE  NAME  :  MMK 

AUTHOR (S)  :  J.  SHEEHAN 

FUNCTION  :  GENERATES  A  DIRECTION  COSINE  MATRIX 

BY  ROTATING  IN  ORDER: 

1)  ANGLE  C  ABOUT  THE  NC  AXIS 

2)  ANGLE  B  ABOUT  THE  NB  AXIS 

3)  ANGLE  A  ABOUT  THE  NA  AXIS 

CALLED  FROM  :  UTILITY  SUBROUTINE 

SUBROUTINES  CALLED  :  ROTMX,  MMLXY 
INPUTS  :  A, NA, B, NB, C, NC 

OUTPUTS  :  RM 

UPDATES  :  D.  SMITH  -  CR  *  59 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (C  Z) 

DIMENSION  AM(3, 3)  ,  BMP, 3),  CMP, 3),  RM(3,3),  T  <  9) 

CALL  ROTMX (A, NA, AM) 

CALL  ROTMX (B, NB, BM) 

CALL  ROTMX (C,NC, CM) 


C 


CALL  MMLXY (BM, CM, T) 
CALL  MMLXY (AM, T, RM) 


o  o  o 
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RETURN 

END 


FILE :  uuv22 . 19g/sutility /uummlxy .for 


c - 

SUBROUTINE  MMLXY(X,Y,Z) 


c 

c 

SUBROUTINE  NAME  : 

MMLXY 

c 

c 

AUTHOR (S)  : 

J.  SHEEHAN 

c 

c 

FUNCTION  : 

MULTIPLY  TWO  3X3  MATRICES 

c 

c 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

c 

c 

SUBPOUTINES  CALLED  : 

NONE 

c 

c 

INPUTS  : 

X,  Y 

c 

c 

OUTPUTS  : 

Z 

c 

c 

UPDATES  : 

D.  SMITH  -  CR  #  59 

c 

C 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 

C 

DIMENSION  X (3, 3) ,  Y<3,3),  Z  <  3 , 3 ) 


Z(I.J)  = 

Z<1,1)  = 
Z (2. 1)  = 
2(3,1)  - 
2(1,2)  « 
2(2,2)  = 
2(3,2)  - 
2(1,3)  - 
Z (2, 3)  = 
2(3,3)  - 
C 

RETURN 

END 


X(I,1)*Y(1,  J) 

X(l,l)  *Y  (1,1) 
X  (2, 1)  *Y  <1, 1) 
X  (3, 1) *Y  <1, 1) 
X  (1,  1)  *Y  (1, 2) 
X  (2, 1)  *Y  (1, 2) 
X(3,1)*Y<1,2) 
X(l,  1)  *Y(1,  3) 
X(2,1)*Y(1,3) 
X  (3,  1)  *Y  (1, 3) 


+  X(I,2)*Y(2,J) 

+  X  (1, 2)  *v  <2, 1) 
+  X  (2, 2)  »Y  (2,  1) 
+  X  (3,  2)  *Y  (2, 1) 
+  X  (1, 2)  *Y  (2, 2) 
+  X  ( 2 ,  2)  «Y  (2,2) 
+  X(3,2)*Y(2,2) 
X(1,2)*Y(2,3) 
+  X(2,2)*Y(2,3) 
+  X(3,2)*Y(2,3) 


X 

(I, 

3) 

*Y 

(3, 

J) 

X 

(1, 

3) 

«Y 

(3, 

1) 

X 

(2, 

3) 

*Y 

(3, 

1) 

X 

(3, 

3) 

*Y 

(3, 

1) 

X 

(1, 

3) 

*Y 

(3, 

2) 

X 

(2, 

3) 

*Y 

(3, 

2) 

X 

(3, 

3) 

*  Y 

(3, 

2) 

X 

(1, 

3) 

*Y 

(3, 

3) 

X 

(2, 

3) 

*Y 

(3, 

3) 

X 

(3, 

3) 

*  Y 

(3, 

3) 

FILE:  uuv22 . 1 9g/sut ility /uuncu . for 


C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 


SUBROUTINE  NCU (DLP, DLY, CMMD, DLPD, DLYD ) 


SUBROUTINE  NAME  : 
AUTHOR (S)  : 

FUNCTION  : 

CALLED  FROM  : 


NCU 

T.  THORNTON 

MODELS  THE  RESPONSE  OF  THE  NOZZLE 
CONTROL  UNIT 

FORTRAN  MAIN 


C  SUBROUTINES  CALLED  :  NONE 

C 

C  INPUTS  :  DLP, DLY, CMMD 

C 

C  OUTPUTS  :  DLPD, DLYD 

C 


c 

UPDATES  : 

D. 

SMITH 

-  CR 

* 

040 

c 

D. 

SMITH 

-  CR 

* 

059 

c 

B. 

HILL  / 

-  CR 

# 

081 

c 

R. 

RHYNE 

c 

B. 

HILL 

-  CR 

# 

093 

C 

C 


IMPLICIT  REAL  (A-H) 
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IMPLICIT  REAL  (O-Z) 

REAL  CMMD ( 2 )  ,  KNCU 

*  DATA  INITIALIZATION 

SINCLUDE ( ' ~ /INCLUDE/SSCON64 . DAT ' ) 

C  PITCH  NOZZLE  GIMBAL  RESPONSE 

DLPD  =  (CMMD(l)  -  KNCU*DLP) *OMEGAT 

C  YAW  NOZZLE  GIMBAL  RESPG..SS 

DLYD  =  (CMMD (2)  -  KNCU*DLY ) *OMEGAT 

C  LIMIT  GIMBAL  RATES 

TOTRAT  =  SQRT  (  DLPD**2  +  DLYD*  *  2  ) 
IF  (  TOTRAT. GT.RMAX  )  THEN 

DLPD  =  DLPD  *  RMAX  /  TOTRAT 

DLYD  =  DLYD  *  RMAX  /  TOTRAT 

END  IF 

RETURN 

END 


FILE:  uuv22.19g/sutility/uunorm.for 


C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

r* 

c 

c 

c 


c 

c 

c 

c 

c 

c 


SUBROUTINE  NORM (SD, MN, ISEED, RDN) 

SUBROUTINE  NAME  :  NORM 

AUTHOR (S)  :  D.  F.  SMITH 

FUNCTION  :  GENERATES  NORMALLY  DISTRIBUTED  RANDOM 

NUMBERS  USING  THE  BOX-MULLER  I ..  ANSFORMATION 

CALLED  FROM  :  UTILITY  SUBROUTINE 

SUBROUTINES  CALLED  :  RANO 

INPUTS  :  SD, MN 

OUTPUTS  :  RDN 

BOTH  :  ISEED 

UPDATES  :  D.  SMITH  -  CR  *  C82 

R.  RHYNE  -  CP.  *  08"? 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

REAL  MN 

INTEGER*  4  ISEED 

SAVE  GSET  ,  ISET 
DATA  GSET / 0 . / ,  ISET/0/ 

DATA  ONE  /  1.0  / 

DATA  TWO  /  2.0  / 

IE  A  SPARE  RANDOM  NUMBER  IS  NOT  AVAILABLE  FROM  THE  PREVIOUS  PASS 
GENERATE  TWO  Nr  K  ONES 

IF  (  ISET.EQ.C  )  THEN 

GET  TWO  .UNIFORM  RANDOM  NUMBERS  WITHIN  THE  SQUARE  EXTENDING 
FROM  -1  TO  1  IN  EACH  DIRECTION 

VI  -  TWO*  RANI  i  I  GEE.)  -  ONE 

V2  -  TWO  * kANS  :  I  SEED )  -  \H 

SEE  i  F  THEY  ARE  W'T'THIN  ’"HE  UNIT  CIRCLE  .  IF  NOT  ,  TRY  AGAIN  . 
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R  =  Vl’Vl  +  V2*V2 

IF  (  R.GE.ONE  )  GO  TO  1 

C  PERFORM  BOX-MULLER  TRANSFORMATION  TO  GENERATE  TWO  GAUSSIAN 

C  RANDOM  NUMBERS  .  RETURN  ONE  AND  SAVE  THE  OTHER  FOR  THE  NEXT 


PASS  , 

FAC 

=  SQRT  (  -TWO’ALOG (R) /R 

GSET 

=  FAC’Vl 

RDN 

=  MN  +  SD*FAC’V2 

ISET 

=  1 

C  USE  GAUSSIAN  RANDOM  NUMBER  CARRIED  OVER  FROM  PREVIOUS  PASS  . 

ELSE  IF  (  ISET.EQ.l  )  THEN 
RON  =  MN  +  SD’GSET 

ISET  =  0 

ENDIF 

RETURN 

END 


FILE :  uuv22 . 19g/ sutility/uuoutmes . for 


SUBROUTINE  OUTMES (N, T, ARG) 
INTEGER  N 
REAL  T,  ARG 

$ INCLUDE ( • : PFP : INCLUDE/TARGET . FOR ' ) 
CHARACTER’ 80  MESSAGE 


C 

C  PROGRAM:  MAIN  (0101 ...  0200) 

C 

IF  (  N.EQ.0101  )  THEN 
WRITE (MESSAGE, 0101)  T 

0101  FORMAT (IX, E16. 9, '  1ST  STAGE  SEPARATION*) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0102  )  THEN 
WRITE (MESSAGE, 0102)  T 

0102  FORMAT (IX, E16. 9, '  2ND  STAGE  SEPARATION*) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0103  )  THEN 
WRITE (MESSAGE,  0103)  T 

0103  FORMAT (IX, E16. 9, '  DROP  NOSE  FAIRING  AND  300ST  ADAPTER’) 
GO  TO  99999 
END  IF 

IF  (  N.EQ.0104  )  THEN 

WRITE (MESSAGE, 0104)  T, ARG 

0104  FORMAT  (1X,E16.9,  1X,E16.9) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0105  )  THEN 

WRITE (MESSAGE, 0105)  T, ARG 

0105  FORMAT  (IX,  E16. 9,  '  MISS  =  *,E16.9) 

GO  TO  99999 
END  IF 


C  SUBROUTINE:  CMPINV  (0201 . . . 0300) 

C 

IF  (  N.EQ.0201  )  1  *EN 
WRITE (MESSAGE, 0201 ) 

0201  FORMAT!'  MATRIX  SIZE  TOO  LARGE  IN  CMPINV) 
GO  TO  99999 
END  IF 


r 

C  SUBROUTINE:  GISCRT  (030 1  .  .  .  OAOC) 

r* 

IF  (  N.EQ.03C1  )  THEN 
WRITE (MESSAGE, 0301 ) 
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0301  FORMAT ('  SYSTEM  ORDER  TOO  LARGE  IN  DISCRT  * ) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0302  )  THEN 
WRITE (MESSAGE, 0302) 

0302  FORMAT ('  SUITABLE  CONVERGENCE  WAS  NOT  REACHED  IN  DISCRT’) 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  EIGVEC  (0901 ...  0500) 

C 

IF  (  N.EQ.0401  )  THEN 
WRITE (MESSAGE, 0901) 

0901  FORMAT!'  MATRIX  SIZE  TOO  LARGE  IN  EIGVEC’) 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  HQR  (0501  ...  0600) 

C 

IF  (  N.EQ.0501  )  THEN 
WRITE (MESSAGE, 0501) 

0501  FORMAT ('  TOO  MANY  ITERATIONS  IN  HQR') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  KALMAN  (0601  ...  0300) 

C 

IF  (  N.EQ.0601  )  THEN 
WRITE (MESSAGE,  0601)  T 

0601  FORMAT (IX, E16. 9, '  INITIATE  ACQUISITION  PHASE ’ > 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0602  )  THEN 
WRITE (MESSAGE, 0602)  T 

0602  FORMAT (IX, E16. 9, '  INITIATE  TRACK  PHASE') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0603  )  THEN 
WRITE (MESSAGE, 0603)  T 

0603  FORMAT ( IX, El  6 . 9,  '  INITIATE  TERMINAL  PHASE') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0604  )  THEN 

WRITE (MESSAGE, 0604)  T , ARG 

0604  FORMAT ( IX , El  6 . 9,  1  ACQUISITION  MODE  ENABLED:  MAGRO  =  '.E16.9) 
GO  TO  99999 
END  IF 

IF  (  N.EQ.0605  )  THEN 

WRITE (MESSAGE, 0605)  T, ARG 

0605  FORMAT (IX, E16. 9, '  TRACK  MODE  ENABLED:  MAGRO  =  ’,E16.9) 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0606  )  THEN 

WRITE (MESSAGE, 0606)  T, ARG 

06C6  FORMAT  (IX,  El  6. 9,  '  C30  MODE  ENABLED:  MAGRO  =  \E16.9) 

GO  TO  99499 
END  IF 

IF  (  N.EQ.C6C7  )  THEN 

WRITE { MESSAGE , C  69  7 )  T, ARG 

0  60  7  FORMAT ( IX, El  6 . 9,  '  TERMINAL  MODE  ENABLED:  MACRO  *  ',EI6.9) 

GO  TO  99999 
END  IF 


,'BPOUT  I NE  :  MATIN’/  (C 
07CI 


:: ...  :900) 


IF  (  N.EQ. 


)  THEN 
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WRITE (MESSAGE, 0701) 

0701  FORMAT  C  MATRIX  SIZE  TOO  LARGE  IN  MATIN'/'  ) 

GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  MCGUID  (0801 ...  0900) 

C 

IF  (  N.EQ.0801  )  THEN 
WRITE (MESSAGE, 0801)  T 

0801  FORMAT (IX, El  6. 9,  '  KV  PITCHOVER  COMPLETE', 

&  '  -  BEGIN  DISTURBANCE  MEASUREMENT') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0802  )  THEN 
WRITE (MESSAGE, 0802)  T 

0802  FORMAT (IX, E16. 9, '  DISTURBANCE  MEASUREMENT  COMPLETE ' , 
&  '  -  ORIENT  KV  TO  LOS') 

GO  TO  99999 
END  IF 

IF  (  N.EQ.0803  )  THEN 
WRITE (MESSAGE, 0803)  T 

0803  FORMAT (IX, E16. 9, '  KV  ORIENTATION  COMPLETE ' ) 

GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  MISSIL  (0901 ...  1000) 

C 

IF  (  N. EQ .0901  )  THEN 
WRITE (MESSAGE, 0901)  T 

0901  FORMAT (IX, E16. 9, '  MISSILE  HAS  CLEARED  THE  LAUNCHER') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  OPTSSC  (1001 ...  1100) 

C 

IF  (  N . EQ .1001  )  THEN 
WRITE (MESSAGE, 1001) 

1001  FORMAT C  MAXIMUM  NUMBER  OF  STATES  EXCEEDED  IN  OPTSSC') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  RAN0  (1101 ...  1200) 

C 

IF  (  N.EQ.1101  )  THEN 
WRITE (MESSAGE, 1101) 

1101  FORMAT C  RANDOM  NUMBER  OUT  OF  BOUNDS  IN  RAN0') 
GO  TO  99999 
END  IF 


C 

C  SUBROUTINE:  SEEKER  ( 1 201 . . . 1 300) 

C 

IF  (  N . EQ .1201  )  THEN 
WRITE (MESSAGE, 1201)  T 

1201  FORMAT (IX, E16. 9, '  TRUE  LOS  ANGLE  EXCEEDS  FIELD-OF-VIEW  LIMIT’) 
GO  TO  99999 

END  IF 

IF  (  N . EQ .1202  )  THEN 
WRITE (MESSAGE, 1202)  T 

1202  FORMAT ( IX , El  6 . 9,  ’  TARGET  REACQUIRED') 

END  IF 

IF  (  N . EQ .1203  )  THEN 

WRITE (MESSAGE, 1203)  T , ARG 

1203  FORMAT (IX, E16. 9, '  FRAME  RATE  CHANGE:  FRMRAT  =  ',£16. 9) 

GO  TO  99999 

END  IF 


non  m  non  *-*  non 
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SUBROUTINE:  SSPLAG  (1301  ...  1400) 

IF  (  N.EQ.1301  )  THEN 
WRITE (MESSAGE, 1301) 

301  FORMAT C  BUFFER  SIZE  INSUFFICIENT  IN  SSPLAG') 
GO  TO  99999 
END  IF 


SUBROUTINE:  TARGET  (1401 ...  1500) 

IF  (  N.EQ.1401  )  THEN 

WRITE (MESSAGE, 1401)  T,  ARG 

401  FORMAT  (IX,  El  6. 9,  '  TARGET  RESOLVED:  RANGE  =  \E16.9) 
GO  TO  99999 
END  IF 


SUBROUTINE:  VCSLOG  ( 1 50 1 . . . 1 600) 

IF  (  N.EQ.1501  )  THEN 

WRITE (MESSAGE, 1501)  T, ARG 

1501  FORMAT (IX, El  6. 9,  '  ISSUE  MIDCOURSE  DISTURBANCE  BURN', 

4  '  -  VCS  THRUSTER  *,F2.0) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1502  )  THEN 

WRITE (MESSAGE, 1502)  T, ARG 

1502  FORMAT (IX, El 6. 9,  '  ISSUE  MIDCOURSE  BURN  ',F2.0) 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1503  )  THEN 

WRITE (MESSAGE, 1503)  T, ARG 

1503  FORMAT (IX, E16. 9, '  ISSUE  MIDCOURSE  BURN  *,F2.0, 

5  '  -  BURN  TIME  BELOW  THRESHOLD') 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1504  )  THEN 
WRITE (MESSAGE, 1504)  T 

1504  FORMAT (IX, El 6. 9,  ■  ISSUE  FIRST  BURN • ) 

GO  TO  99999 

ENO  IF 

IF  (  N.EQ.1505  )  THEN 
WRITE (MESSAGE, 1505)  T 

1505  FORMAT (IX, E16. 9, '  ISSUE  FIRST  BURN', 

S  '  -  BURN  TIME  BELOW  THRESHOLD') 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1506  )  THEN 
WRITE (MESSAGE,  1506)  T 

1506  FORMAT (IX, E16. 9, '  ISSUE  SECOND  BURN') 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1507  )  THEN 
WRITE (MESSAGE, 1507)  T 

1507  FORMAT ( IX, El  6 . 9,  '  ISSUE  SECOND  BURN', 

4  '  -  BURN  TIME  BELOW  THRESHOLD') 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1508  )  THEN 
WRITE (MESSAGE, 1508)  T 

1508  FORMAT (1X.E16. 9, '  ISSUE  THIRD  BURN') 

GO  TO  99999 

END  IF 

IF  (  N.EQ.1509  )  THEN 
WRITE (MESSAGE, 1509)  T 

150  9  FORMAT (IX, El  6. 9,  '  ISSUE  THIRD  BURN', 

'  -  BURN  TIME  3ELUW  THRESHOLD') 

GO  TO  99999 
END  IF 


4 
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WRITE (MESSAGE, 0001)  N 

0001  FORMAT ('  ERROR:  MESSAGE  NUMBER  =  ',14) 


99999  CONTINUE 

CALL  OUTPUT_MESSAGE (  %VAL (CHARACTER_08BIT) ,  MESSAGE  ) 
CALL  OUTPUT_NL 

RETURN 

END 


FILE:  uuv22 . 19g/sut ility/uuran .for 


c - 

REAL  FUNCTION  RAN (ISEED) 


c 

c 

c 

SUBROUTINE  NAME  : 

RAN 

c 

c 

AUTHOR (S)  : 

D.  F.  SMITH 

c 

c 

c 

FUNCTION  : 

GENERATES  A  UNIFORMLY  DISTRIBUTED  RANDOM 
NUMBER 

c 

c 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

c 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

c 

INPUTS  : 

NONE 

C 

c 

OUTPUTS  : 

RAN 

c 

c 

BOTH  : 

ISEED 

c 

c 

c— 

UPDATES  : 

NONE 

INTEGER* 4  ISEED 

iseed  -  69069*iseed  +  1 

ran  ■  abs  ( float  ( iseed) /214"M83 64 7 . 0 ) 

RETURN 

END 


FILE :  uuv22 . 1 9g/su*  < ’ i ty/uuranO .for 


REAL  FUNCTION  RAN0 (ISEED) 

C 

c 

c 

SUBROUTINE  NAME  : 

RAN0 

c 

c 

AUTHOR (S)  : 

D.  F.  SMITH 

c 

FUNCTION  : 

GENERATES  A  UNIFORMLY 

DISTRIBUTED  RANDOM 

c 

NUMBER  BETWEEN  0  AND 

1  USING  THE  SYSTEM 

c 

ROUTINE  RAN (ISEED)  . 

THE  BUFFER  IN  COMMON 

c 

c 

c 

BLOCK  RANCOM  IS  INITIALIZED  BY  CALLING 
ROUTINE  PANIT  . 

c 

c 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

c 

c 

SUBROUTINES  CALLED  : 

RAN 

c 

c 

INPUTS  : 

NONE 

c 

c 

OUTPUTS  : 

RAN0 

c 

c 

BOTH  : 

ISEED 

c 

c 

r - 

UPDATES  : 

NONE 

C 


no  on 
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IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

INTEGER*  4  ISEED 

COMMON  /  RANCOM  /  RANSEQ ( 97 ) ,  RANLST 

USE  PREVIOUSLY  SAVED  RANDOM  NUMBER  AS  BUFFER  INDEX  AND  MAKE 
SURE  ARRAY  BOUNDS  ARE  NOT  EXCEEDED  . 

J  =  1  +  INT  (  97 . 0*RANLST  ) 

IF  (  J.LT.l  .OR.  J.GT.97  )  THEN 
CALL  OUTMES (1100, 0. 0,0.0) 

END  IF 

RETRIEVE  RANDOM  NUMBER  FROM  3UFFER  FOR  OUTPUT  AND  SAVE  IT  FOR 
USE  AS  AN  INDEX  ON  THE  NEXT  PASS  . 

RANLST  =  RANSEQ (J) 

RAN0  =  DBLE  (  RANLST  ) 

RAN0  =  RANLST 

C  LOAD  A  NEW  RANDOM  NUMBER  IN  THE  SLOT  JUST  VACATED  . 

RANSEQ (J)  =  RAN  (  ISEED  ) 

RETURN 

END 


FILE :  uuv22 . 19g/sutility/uu ranit .for 


SUBROUTINE  RANIT  ( 

ISEED  ) 

SUBROUTINE  NAME  : 

RANIT 

AUTHOR (S)  : 

D.  F.  SMITH 

FUNCTION  : 

INITIALIZES  A  TABLE  OF  RANDOM  NUMBERS  FOR 

USE  BY  THE  UNIFORM  RANDOM  GENERATOR  RAN0 

CALLED  FROM  : 

EXECUTIVE  ROUTINE 

SUBROUTINES  CALLED 

:  RAN 

INPUTS  : 

NONE 

OUTPUTS  : 

NONE 

BOTH  : 

ISEED 

UPDATES  : 

NONE 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 

INTEGEK*4  ranit 

COMMON  /  RANCOM  /  RANSEQ (97),  RANLST 

C  EXERCISE  SYSTEM  ROUTINE 

DO  1  ^  I  -  1  ,  97 

DUMMY  «  RAN  (  ISEED  ) 

10  CONTINUE 

C  STORE  97  RANDOM  NUMBERS  IN  BUFFER  (  97  Is  NOT  SPECIAL  ) 

DO  20  I  -  1  ,  97 

RANSEQ (I)  =  RAN  (  ISEED  ) 

20  CONTINUE 

C  SAVE  ANOTHER  RANDOM  NUM3ER  TO  USE  FOR  INDEXING  BUFFER 


RANLST 


RAN  (  ISEED  ) 


nnonnnnunnnonnoonnnnnonnnnonnoon 
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RETURN 

END 

FILE:  uuv22 . 19g/sutility/uuresp2r. for 

C - 

SUBROUTINE  RESP2R  (  DT, WD, 2D, CILL, OIL, Cl , COLL, COL, CO  ) 


SUBROUTINE  NAME  :  RESP2R 


AUTHOR (S)  :  D.  F.  SMITH 

FUNCTION  :  Given  a  second  order  continuous  filter  of 

the  form 


G(s) 


WD**2 


s**2  +  2 . 0*ZD*WD*  s  +  WD**2 


compute  a  digital  filter  which  yields  the 
same  ramp  response  .  The  digital  filter  has 
the  transfer  function 


G(z) 


CI*Z**2  +  CIL*  z  +  CILL 


CO*z**2  +  COL*z  +  COLL 


CALLED  FROM  : 
SUBROUTINES  CALLED 
INPUTS  : 

OUTPUTS  : 

UPDATES  : 


UTILITY  ROUTINE 
NONE 

DT, WD, ZD 

CILL,  CIL, Cl, COLL, COL, CO 
NONE 


IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (O-Z) 


DATA  ONE  /  1.0  / 

DATA  TWO  /  2.0  / 


C  Underdamped  filter 

IF  (  ZD.LT.ONE  )  THEN 


A 

WD*ZD 

B 

= 

WD'SQRT  (  ONE  -  ZD**2  ) 

TMP1 

= 

EXP  (  -  A*DT  ) 

TMP2 

= 

EXP  (  -  TWO*A*DT  ) 

TMP3 

= 

CO.  (  B*DT  ) 

TMP  4 

* 

SIN  (  B*DT  ) 

TMP  5 

A*A  +  B*B 

TMP  6 

= 

TMP 1* TMP 4* (  A*A  -  B*B  )/B 

Cl 

= 

TMP5*DT  -  TWO*A  <•  TWO*A*TMPl *TMP3  t  TMP 6 

CIL 

TWO* (  A  -  DT*TMP1*TMP3*TMP5  -  TMP 6  -  A*TMP2  ) 

CILL 

= 

TMP 6  -  TWO* A* TMP 1 *TMP3  *  TMP2*(  TWO*A  t  TMP5*DT 

CO 

= 

TMP5*DT 

COL 

= 

-  TWO* TMP 1  * TMP 3  * CO 

COLL 

= 

TMP2*CO 

END  IF 


C  Critically  damped  filter 


IF  (  ZD.EQ.UNE  )  THEN 


A 

* 

WD 

TMP1 

= 

EXP  ( 

-  A*DT  ) 

TMP2 

= 

EXP  ( 

-  TWO* A* DT  ) 

TMP  3 

= 

TWO  + 

A*DT 

TMP4 

= 

-  TWO  * 

A*DT 

Cl 

TMP1  * 

TMP 3  +  TMP  ^ 

CIL 

= 

TWO*  ( 

ONE  -  TWO* A*DT*' 

CILL 

* 

TMP  1  * 

TMP  4  +  TMP 2* TMP 3 

CO 

* 

A*DT 

COL 

= 

-  CO'TWO'TMPl 

TMP2  ) 
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COLL  =  CO*TMP2 
END  IF 

C  Overdamped  filter 


IF  (  ZD.GT.ONE  )  THEN 


TMP5 

= 

SQRT  (  ZD* *2  -  ONE  ) 

A 

= 

WD*TMP5 

B 

= 

WD/TMP5 

ASQ 

= 

A*A 

BSQ 

= 

B*B 

EXPA 

5= 

EXP  (  -  A*DT  ) 

EXPB 

* 

EXP  (  -  B*DT  ) 

TMP1 

= 

A*DT  +  EXPA  -  ONE 

TMP2 

= 

B*DT  +  EXPB  -  ONE 

TMP3 

= 

ONE  +  A*DT 

TMP4 

= 

ONF.  +  B*DT 

Cl 

ASQ*TMP2  -  BSQ*TMP1 

CIL 

= 

ASQ* (  ONE  -  EXPA*TMP2 

-  EXPB*TMP4 

-  BSQ* (  ONE  -  EXPB*TMP1 

-  EXPA*TMP3 

CILL 

a 

ASQ*EXPA* (  EXPB*TMP4  - 

ONE  ) 

-  BSQ*EXPB*(  EXPA*TMP 3  - 

ONE  ) 

CO 

* 

A*B*DT* (  A  -  B  ) 

COL 

= 

-  CO* (  EXPA  +  EXPB  ) 

COLL 

a 

CO* EXPA* EXPB 

END  IF 

RETURN 

END 

FILE :  uuv22 . 19g/sutility/uurotmx. for 


SUBROUTINE  ROTMX(X,I, 

,  XM) 

SUBROUTINE  NAME  : 

ROTMX 

AUTHOR  (S)  : 

J.  SHEEHAN 

FUNCTION  : 

GENERATES  A  DIRECTION  COSINE  MATRIX 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

SUBROUTINES  CALLED  : 

NONE 

INPUTS  : 

X,  I 

OUTPUTS  : 

XM 

UPDATES  : 

D.  SMITH  -  CR  #  59 

IMPLICIT  REAL  (A-H) 
IMPLICIT  REAL  (O-Z) 
REAL  XM (3, 3) 

SX  *  SIN (X) 

CX  =  COS  (X) 

IF  (  I.EQ.l  t  THEN 

XM  (1,1)  =  1.0 

XM  (1,2)  =  0.0 

XM  (1,3)  =  0.0 

XM (2,1)  =  0.0 

XM (2 , 2 )  =  CX 

XM  (2.3)  =  SX 

XM (3,1)  =0.0 

XM ( 3 , 2)  =  -SX 

XM  (3.3)  -  CX 

END  IF 

IF  (  I.EQ.2  )  THEN 
XM(1, 1)  =  CX 

XM  (1,2)  =  0.0 

XM  (1,3)  =  -SX 

182  Annual  Report:  Digital  Emulation  Technology  Laboratory  Volume  1,  Part  3 


XM  (2,1) 

0.0 

XM (2,2) 

= 

1.0 

XM  (2,3) 

= 

0.0 

XM  (3,1) 

= 

SX 

XM (3,2) 

= 

0 . 0 

XM  (3,3) 
END  IF 

* 

cx 

IF  (  I.EQ. 

.3 

)  THEN 

XM (1,1) 

* 

CX 

XM(1, 2) 

= 

SX 

XM  (1,3) 

= 

0.0 

XM (2,1) 

= 

-SX 

XM  (2,2) 

* 

cx 

XM (2, 3) 

- 

0.0 

XM  (3,1) 

= 

0.0 

XM (3, 2) 

* 

0.0 

XM (3, 3) 

= 

1.0 

END  IF 


RETURN 

END 


FILE :  uuv22 . 19q/ sut il i ty/uuseeker . for 


C - 

SUBROUTINE  SEEKER (T, ACQD, LAMSEK, MAGRTR,  SKSEED, FRMRAT, FRMCNT, 
SAMRAT, TRACK, TERM, SNR,  LAMM) 

C - 

t  SUBROUTINE  NAME  :  SEEKER 

C 

C  AUTHOR (S)  :  M.  K.  DOUBLEDAY,  D.  C.  FOREMAN 

C 

C  FUNCTION  :  SEEKER  MODEL 

C 

C  CALLED  FROM  :  FORTRAN  MAIN 

C 

C  SUBROUTINES  CALLED  :  NORM,  TABLE 

C 

C  INPUTS  :  T, ACQD, LAMSEK, MAGRTR 

C 

C  OUTPUTS  :  SAMRAT, TRACK, TERM, SNR, LAMM 

C 

C  BOTH  :  SKSEED, FRMRAT, FRMCNT 

C 


c 

UPDATES  : 

T. 

THORNTON 

- 

CR 

* 

014 

c 

B. 

HILL 

- 

CR 

* 

020 

c 

D. 

SMITH 

- 

CR 

# 

027 

c 

B. 

HILL 

- 

CR 

* 

030 

c 

B. 

HILL 

- 

CR 

# 

038 

c 

T. 

THORNTON 

- 

CR 

* 

043 

c 

T. 

THORNTON 

- 

CR 

# 

044 

c 

T. 

THORNTON 

- 

CR 

i 

048 

c 

D. 

SISSOM 

- 

CR 

# 

053 

c 

D. 

SMITH 

- 

CR 

# 

059 

c 

D. 

SMITH 

- 

CR 

# 

064 

c 

D. 

SISSOM 

- 

CR 

# 

069 

c 

D. 

SMITH 

- 

CR 

# 

074 

c 

D. 

SMITH 

- 

CR 

# 

080 

c 

B. 

HILL  / 

- 

CR 

» 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH 

- 

CR 

* 

082 

c 

R. 

RHYNE 

- 

CR 

# 

084 

c 

R. 

RHYNE 

- 

CR 

* 

087 

z 

R. 

RHYNE 

- 

CR 

* 

088 

c 

B. 

HILL 

- 

CR 

* 

093 

C 

c - 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  (0-Z) 

REAL  ACQRNG (4,4)  ,  LAMB (2)  ,  LAMFOV 

REAL  LAMM (2)  ,  LAMNEA ( 2 )  ,  LAMSEK (2) 
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REAL  LAMSK (2)  ,  MAGRTR 

REAL  NEA  ,  RATE (6)  ,  S£KNOS!24) 

REAL  SEKTIM  (24)  ,  TRGSIGM) 

INTEGER  ACQD  ,  BCKGRD  ,  FRMCNT 

INTEGER  SEKTYP 

INTEGER  TERM  ,  TRACK 

INTEGER*  4  SKSEED 

C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  ISEKR,  IFOV 

*  DATA  INITIALIZATION 

S INCLUDE ( ' "/ INCLUDE /SSCON4 7 . DAT  1 ) 

SINCLUDE  (  '  -VINCLUDE/SSCON‘18.DAT,  ) 

$  INCLUDE ( 1 ^/INCLUDE/SSCONSC . DAT ' ) 

SINCLUDE  (  '  ‘'/INCLUDE/SSCON55  .  DAT  •  ) 

SINCLUDE ( ’ A /INCLUDE/S SCON 61 . DAT • ) 

SINCLUDE < • VINCLUDE/SSCON68 . DAT  1 ) 

SINCLUDE  ( ’ VINCLUUE/SSCONIO.DAT1  ) 

SINCLUDE ( ’ "/INCLUDE/SSCON1 1 .DAT1 ) 

DATA  ISEKR  /  1  / 

DATA  IFOV  /  0  / 

DATA  IT  /  1  / 

IF  (ISEKR. EQ.l)  THEN 

ISEKR  =  0 

IF  (  SEKTYP.EQ.2  )  THEN 
TSIG  =  TRGSIG ( ITRGSG) 

TSGACQ  =  TSIG 

RAQREF  »  ACQRNG (BCKGRD, ITRGSG) 

RNGAQ  =  SORT ( (TSGACQ/TSIG) * <6. 0/SNRACQ) * 

(SQRT (1 . /FRMRAT) ) ) *RAQREF 

END1F 

ENDIF 

C  TEST  FOR  FIELD-OF-VIEW  LIMIT 

IF  (  SEKTYP.EQ.2  .AND.  SNR . GE . GNRACQ  )  THEN 
FOVCHK  =  FOVLIM 

ELSE  IF  (  ACQD. EQ.l  .ANJ.  SEKTYP.NE.2  )  THEN 
FOVCHK  =  FOV 
ELSE 

FOVCHK  =  1000. 

ENDIF 

LAMFOV  =  AMAX1 (  LAMSEK ( 1 )  ,  .  LAMSEK  (2)  ) 

IF  (  LAMFOV. GE. FOVCHK  .AND.  IFOV.EQ.O  )  THEN 
CALL  OUTMES (1201, T, 0. 0) 

IFOV  =  1 

ELSE  IF  (  LAMFOV. LE. FOVCHK  .AND.  IFOV. EQ.l  )  THEN 
CALL  OUTMES (1202, T, 0.0) 

IFOV  =  0 
ENDIF 

C  DETERMINE  SEEKER  SAMPLE  RATE  FOR  SEEKER  TYPES  0  AND  1 

*  IF  (  SEKTYP.EQ.O  .OR.  SEKTYP .EQ.l  )  THEN 

*  IF  (  MAGRTR  .LE.  RNGTRM  )  THEN 

*  SAMRAT  =  SAMTRM 

*  IF  ( TERM . EQ . 0 )  TERM  =  1 

*  ELSE  IF  (  MAGRTR  . LE .  RNGTRK  )  THEN 

*  SAMRAT  =  SAMTRK 

*  IF  (TRACK. EQ.0)  TRACK  =  1 

*  ELSE 

*  SAMRAT  =  SAMACQ 

*  ENDIF 

*  ENDIF 

C  PERFECT  SEEKER  MODEL 

*  IF  (  SEKTYP.EQ.O  )  THEN 

*  LAMM  ( 1 )  =  LAMSEK  (I) 

*  LAMM (2)  =  LAMSEK (2) 

*  FRMRAT  =  1.0/ SAMRAT 

*  ENDIF 


no  no 
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C  SIMPLE  SEEKER  MODEL 

*  IF  (  SEKTYP.EQ.l  )  THEN 

*  FRMRAT  =  1.0/ SAMRAT 

*  CALL  NORM (1.0, 0. 0. SKSEED, RAN A) 

*  CALL  TABLE (SEKTIM, SEKNOS, T, SKNOSA, 24, IT) 

*  LAMSK(l)  =  LAMSEK ( 1 )  +  0 . 001*RANA*SKNOSA 

*  CALL  NORM (1.0, 0.0, SKSEED, RANB) 

*  CALL  TABLE (SEKTIM, SEKNOS, T, SKNOSB, 24, IT) 

*  LAMSK (2)  =  LAMSEK (2)  +  0 . 001*RANB'SKNOSB 

C  ANGLE  QUANTIZATION 

*  IF  (  QNTZP  -GT.  0.  )  THEN 

*  LAMM ( 1 )  =  (AINT (LAMSK (1) ) /QNTZP  +  O.S)*QNTZP 

*  LAMM ( 2 )  =  (AINT (LAMSK (2) ) /QNTZP  +  0.5)*QNTZP 

*  ELSE 

*  LAMM ( 1 )  -  LAMSK (1) 

*  LAMM ( 2 )  -  LAMSK (2) 

*  END  IF 

*  ENDIF 

C  SEEKER  MODEL  DEPENDENT  ON  RANGE,  FRAME,  AND  ENVIRONMENT 

IF  (  SEKTYP.EQ.2  )  THEN 

C  DETERMINE  THE  SIGNAL-TO-NOISE  RATIO 

IF  (  MAGRTR . LE . RFINAL  )  THEN 

SNR  =  (RAQREF**2/RFINAL**2) * (TSGACQ/TSIG) * 

(SQRT (1 . 0/FRMRAT) ) *SNRACQ 

ELSE 

SNR  »  (RAQREF**2/MAGRTR**2) * (TSGACQ/TSIG) * 

(SQRT (1.0 /FRMRAT) ) *SNRACQ 

ENDIF 

CALCULATE  THE  NOISE  EQUIVALENT  ANGLE  (RADIANS)  FROM  THE 
EFFECTIVE  SNR 

NEA  =  (32. S6*SNR*« (-0.29912) ) *1.0E-6 

MULTIPLY  NOISE  EQUIVALENT  ANGLE  BY  NORMALLY  DISTRIBUTED  RANDOM 
VARIABLE  WITH  A  MEAN  OF  ZERO  AND  A  STANDARD  DEVIATION  OF  ONE 

CALL  NORMU.  0,0.0,  SKSEED,  RANA) 

CALL  NORM (1.0, 0.0, SKSEED, RANB) 

LAMNEA(l)  =  NEA* RANA 
LAMNEA (2)  =  NEA*RANB 

C  DETERMINE  MEASURED  LOS  ANGLE  (RADIANS) 

LAMB ( 1 )  =  LAMSEK (1)  +  LAMNEA ( 1 ) 

LAMB (2)  =  LAMSEK (2)  +  LAMNEA (2) 

C  QUANTIZE  THE  MEASURED  LOS  ANGLE  (RADIANS) 

IF  (  QNTZP. GT. 0.0  )  THEN 

LAMM ( 1 )  =  (AINT (LAMB (1) /QNTZP  *  0.5))*QNTZP 
LAMM ( 2 )  -  (AINT (LAMB (2) /QNTZP  +  0.5))*QNTZP 
ELSE 

LAMM  ( 1 )  =  LAMB  ( 1 ) 

LAMM (2)  =  LAMB (2) 

ENDIF 

C  DETERMINE  IF  A  FRAME  RATE  SWITCH  IS  REQUIRED 

IF  (  MAGRTR. LE. RFINAL  )  THEN 

FRMR  =  (  (6.0/SNRMIN) * (TSGACQ/TSIG) *  (RAQREF* * 2/RFINAL* *  2 ) ) **2 
ELSE 

FRMR  =  (  (6.0/SNRMIN) * (TSGACQ/TSIG) * <RAQREF* *2 /MAGRTR* *  2 )  ) **2 
ENDIF 

IF  (  FRMR. GE. RATE (FRMCNT)  .AND.  FRMCNT.LT.7  )  THEN 
FRMRAT  =  RATE (FRMCNT) 

FRMCNT  =  FRMCNT  +  1 
CALL  OUTMES (1203, T, FRMRAT) 

ENDIF 

ENDIF 


RETURN 
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END 


FILE :  uuv22 .19g/sutility/uussplag.for 


SUBROUTINE 

S SPLAG (T, LAMM, RREL, VREL, TI2M, SNR, LATCH, KFSF, TKFU, 

LAMMO, RRELO, VRELO, TI2MO, SNRO) 

c 

c 

SUBROUTINE 

NAME  :  SSPLAG 

c 

AUTHOR (S)  : 

D.  F.  SMITH 

c 

FUNCTION  : 

Emulate  the  signal  processing  lag  which 

c 

occurs  between  the  seeker  signal  processor 

c 

input  and  output. 

c 

CALLED  FROM 

:  FORTRAN  MAIN 

c 

SUBROUTINES 

CALLED  :  NONE 

c 

INPUTS  : 

T, LAMM, RREL, VREL,  TI2M,  SNR, LATCH 

c 

OUTPUTS  : 

KFSF, TKFU, LAMMO, RRELO, VRELO, TI2MO, SNRO 

c 

UPDATES  : 

D.  SISSOM  -  CR  #  069 

c 

B.  HILL  /  -  CR  »  081 

c 

R.  RHYNE 

c 

c 

R.  RHYNE  -  CR  #  087 

PARAMETER  <NSAVMX=10) 


IMPLICIT  REAL 

(A-H) 

IMPLICIT  REAL 

(O-Z) 

REAL 

LAMM (2) , 

LAMMO 

REAL 

RREL (3) , 

RRELO 

REAL 

VRELO)  , 

VRELO 

REAL 

TI2M ( 9) , 

TI2MO 

REAL 

SNR, 

SNRO, 

REAL 

TLATCH (NSAVMX) 

(2) ,  LAMMSV (2, NSAVMX) 

(3) ,  RRELSV (3, NSAVMX 1 

(3).  VRELSV (3, NSAVMX) 

(9),  TI2MSV( 9, NSAVMX) 

SNRSV (NSAVMX) 


*  DATA  INITIALIZATION 

SINCLUDE ( ' * / INCLUDE /SSSSP LAG . DAT ' ) 

S INCLUDE ( ■"/INCLUDE/SSCON56.DAT1 ) 

C  ENSURE  THAT  BUFFER  BOUNDARIES  ARE  NOT  VIOLATED 


IF  (  LATCH. GT.O  )  THEN 
N LATCH  =  N LATCH  +  1 
IF  (  NLATCH.GT. NSAVMX  )  THEN 
CALL  OUTMES (1301, 0. 0, 0. 0) 
ENDIF 
ENDIF 


C  LATCH  DATA  INTO  BUFFER  AT  MEASUREMENT  TIME 


IF  (  LATCH. GT.O  )  THEN 
TLATCH  (NLATCH) 
LAMMSV ( 1 , NLATCH)  » 
LAMMSV  (2,  NLATCH)  = 
RRELSVU,  NLATCH)  = 
RRELSV (2, NLATCH)  » 
RRELSV (3, NLATCH)  « 
VRELSV (1, NLATCH)  = 
VRELSV (2, NLATCH)  = 
VRELSV (3, NLATCH)  = 
TI2MSV(1,  NLATCH)  = 
TI2MSV(2, NLATCH)  = 
TI2MSV  (3, NLATCH)  - 
TI2MSV  (4,  NLATCH)  --- 
TI2MSV (5, NLATCH)  = 
TI2MSV(6, NLATCH)  = 
TI2MSV(7,  NLATCH)  - 
TI2MSV(8, NLATCH)  = 
TI2MSVI9, NLATCH)  » 
SNRSV (NLATCH) 


T  +  SPLAG 
LAMM  ( 1 ) 
LAMM (2) 
RREL (1) 
RREL  (2) 
RREL (3) 
VREL (1) 
VREL ( 2 ) 
VREL (3) 
TI2M (1) 
TI2M (2) 
TI2M (3) 
TI2M ( 4) 
TI2MI5) 
TI2M  (  6) 
TI2M<7) 
TI2M (3) 
TI2M (9) 
SNR 
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ENDIF 

C  UNLATCH  DATA  FROM  BUFFER  AT  KALMAN  FILTER  PROCESSING  TIME 

IF  (  LATCH. LT.O  )  THEN 
LAMMO(l)  =  LAMMS  V(  1,1) 

LAMMO ( 2 )  =  LAMMSV(2,1) 

RRELO(l)  =  RRELSV (1,1) 

RRELO ( 2 )  =  RRELSV (2,1) 

RRELO (3)  =  RRELSV (3,1) 

VRELO (1)  =  VRELSV (1, 1) 

VRELO (2)  =  VRELSV (2,1) 

VRELO (3)  =  VRELSV (3, 1) 

TI2MO (1 )  «  TI 2MSV ( 1 , 1) 

TI2MO (2)  =  TI2MSV (2, 1) 

TI2MO ( 3 )  =  TI2MSV (3,1) 

TI  2MC  (  4  )  «  TI2MSV(4,1) 

TI2MO ( 5)  =  TI2MSV (5,1) 

TI2MO ( 6)  =  TI2MSV ( 6 , 1) 

TI2M0 (7)  =  TI2MSV (7, 1) 

TI2MO ( 8 )  =  TI2MSV (8,1) 

TI2MO (9)  »  TI2MSV ( 9, 1) 

SNRO  =  SNF'-V(l) 

ENDIF 

C  ALTER  BUFFER  CONTENTS  WHEN  DATA  IS  UNLATCHEL 

IF  (  LATCH. LT.O  )  THEN 
DO  20  I  =  1  ,  NLATCH -1 

TLATCH (I )  =  TLATCH (I  +  l ) 

LAMMSV (1,1)  =  LAMMSV (1,1+1) 

LAMMSV (2,1)  =  LAMMSV (2, I  +  l ) 

RRELSV (1,1)  =  RRELSV (1, I+l) 

RRELSV  (2,1)  =  RRELSV12,  I  +  l) 

RRELSV  (3,1)  =  RRELSV(3,  I  +  l) 

VRELSV (1,1)  =  VRELSV (1, I+l) 

VRELSV (2,1)  =  VRELSV(2, I+l) 

VRELSV  (3,1)  =  VRELSV  (3,  I  +  l) 

TI2MSV  (1,1)  =  TI2MSVU,  I  +  l) 

TI2MSV (2,1)  =  TI2MSV(2, I+l) 

TI 2MSV (3,1)  =  TI2MSV (3, I+l) 

TI2MSV  (4,1)  =  TI2MSVM,  I  +  l) 

TI 2MSV (5,1)  «  TI2MSV (5, I+l) 

TI2MSV (6,1)  -  TI2MSV (6, I+l) 

TI  2MSV  (7,1)  -  TI2MSV(7,  I  +  l) 

TI 2MSV (8,1)  =  TI2MSV (8, I+l) 

TI  2MSV  (9,1)  =  TI2MSV(9,  I  +  l) 

SNRSV(I)  =  SNRSV  (I  +  l ) 

20  CONTINUE 

TLATCH  (NLATCH)  =  0.0 
LAMMSV (1, NLATCH)  -  0.0 
LAMMSV  (2,  NLATCH)  =  0.0 
RRELSV (1, NLATCH)  =  0.0 
RRELSV (2, NLATCH)  =0.0 
RRELSV  (3,  NLATCH)  =0.0 
VRELSV  (1  ,  NLATCH)  =  0.0 
VRELSV (2, NLATCH)  =  0.0 
VRELSV (3, NLATCH)  =  0.0 
TI2MSV (1, NLATCH)  =  0.0 
TI2MSV(2, NLATCH)  =  0.0 
TI2MSV (3, NLATCH)  =  0.0 
TI2MSV (4, NLATCH)  =  0.0 
TI2MSV (5, NLATCH)  =  0.0 
TI2MSV(  6,  NLATCH)  =  0.0 
Tr2MSV(  7,  NLATCH)  =  0.0 
TI2MSV (8, NLATCH)  =  0.0 
TI2MSV (9, NLATCH)  =  0.0 
SNRSV (NLATCH)  =  0.0 
NLATCH  =  NLATCH  -  1 

ENDIF 

C  DETERMINE  TIME  OF  NEXT  KALMAN  FILTER  UPDATE  AND  ENABLE  KALMAN 

C  FILTER  SCHEDULING  FLAG  AS  NEEDED 


(  LATCH 

.  GT  .  0  .AND.  NLAT’ 

TKFU 

=  TLATCH  (1) 

KFSF 

=  1 

.SE  IF  ( 

1ATCH.LT. 3  .AND. 

TKFU 

=  TLATCH (1) 

KFSF 

-  1 

ELSE 


n  n  n 
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KFSF  =  0 
ENDIF 

RETURN 

END 


FILE :  uuv22 . 19g/sutility/uu table. for 


C 


SUBROUTINE  TABLE (XTAB, 

YTAB, X, Y, N, I) 

c 

c 

c 

SUBROUTINE  NAME  : 

TABLE 

c 

AUTHOR (S)  : 

D.  SMITH 

c 

FUNCTION  : 

PERFORMS  TABLE  LOOKUP  VIA  EITHER  INDEXED 

c 

SEARCH  CR  BINARY  SEARCH  AND  LINEARLY 

c 

c 

INTERPOLATES 

c 

c 

CALLED  FROM  : 

UTILITY  SUBROUTINE 

c 

c 

SUBROUTINES  CALLED  : 

NONE 

c 

c 

INPUTS  : 

XTAB, YTAB, X, N 

c 

c 

OUTPUTS  : 

Y 

c 

c 

BOTH  : 

I 

C 

UPDATES  : 

D.  SMITH  -  CR  #  27 

c 

B.  HILL  -  CR  #  38 

c 

B.  HILL  -  CR  #  46 

c 

c 

D.  SMITH  -  CR  #  59 

IMPLICIT  REAL  (A-H) 
IMPLICIT  REAL  (0-Z) 
INTEGFD  N, I 
REAL  XTAB (N)  , YTAB (N) 
C 


(  I.GE.l 

•AND.  I.LE.N  ) 

THEN 

IF  (  X.LE 

. XTAB (1 )  )  THEN 

Y 

=  YTAB ( 1 ) 

I 

=  1 

ELSE  IF  ( 

X.GE.XTAB(N)  ) 

THEN 

Y 

=  YTAB (N) 

I 

=  N 

ELSE  IF  ( 

X.GE.XTAB(I)  ) 

THEN 

DO  10 

K  =  I  ,  N-l 

IF  (  X.LT.XTAB(K+1)  ) 

GO 

TO  20 

10 

CONTINUE 

20 

FRACT  =  (  X  -  XTAB (K)  ) 

/  ( 

XTAB (K+l ) 

-  XTAB  (K) 

Y  -  YTAB (K)  +  FRACT 

I  =  K 

*  ( 

YTAB (K+ 1 ) 

-  YTAB (K ) 

ELSE  IF  (  X.LT.XTAB(I)  )  THEN 

DO  30  K  =  1-1  ,  1  ,  -1 

IF  (  X.GE.XTAB(K)  )  GO  TO 

40 

30 

CONTINUE 

40 

FRACT  =*  (  X  -  XTAB  (K)  ) 

/  ( 

XTAB (K+l! 

-  XTAB { K) 

Y  =  YTAB (K)  +  FRACT 

*  ( 

YTAB (K+l ) 

-  YTAB  (K ) 

I  =  K 

END  IF 


PERFORM  BINARY  SEARCH  IF  POINTER  IS  ZERO  OR  OUT  OF  BOUNDS 

ELSE  IF  (  I.LT.l  .OR.  I.GT.N  )  THEN 

IF  (  X.GT.XTAB(l)  .AND.  X.LT.XTAB(N)  )  THEN 
K  =  1 

L  ■  N 

DO  50  I  =  K  ,  L 

IF  (  L.EQ.K+1  )  GO  TO  60 
M  =  (  K  +  L  )  /  2 

IF  (  X.LT.XTAB(M)  )  THEN 
L  =  M 

ELSE 

K  =  M 

END  IF 
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50  CONTINUE. 

60  FRACT  -  (  X  -  XTAB(K)  )  /  (  XTAB(L) 


Y 

=  YTAB(K)  + 

FRACT  * 

I 

=  K 

ELSE 

IF 

(  X.LE.XTAB(l) 

)  THEN 

Y 

=  YTAB(l) 

I 

=  1 

ELSE 

IF 

(  X.GE.XTAB(N) 

)  THEN 

Y 

=  YTAB(N) 

I 

=  N 

END  IF 
END  IF 

RETURN 

END 


XTABIKj  ) 
YTAB(K)  ) 


FILE :  uuv22 . 1 9g/sut il ity /uut irae r . f or 


SUBROUTINE  INITIALIZE  JTIMER ( ) 
SINCLUDE ( • : PFP: INCLUDE/TARGET. FOR' ) 
SINCLUDE  (  '  VINCLUDE/UUTIMER.COM'  ) 
SINCLUDE ( '  VINCLUDE/SSCON22 . DAT ' ) 
SINCLUDE ( ' * / INCLUDE /SSCON2  3 . DAT ' ) 

INTEGER  BN,  TN 

DO  20  BN=1, 4 
DO  10  TN=1, 500 

NUMBERJTIMER  (BN,  TN)  =  0 
NUMBER_TICKS (BN, TN)  =  0.0 
10  CONTINUE 

20  CONTINUE 

STAGE1  =  INT4  (  TSTG1  *  1000. C  ; 

STAGE2  =  INT4  (  TSTG2  *  1000. 0  ) 

CALL  RESET  TIMER  0 

END 

SUBROUTINE  START  TIMER (  TN  ) 
SINCLUDE (' :PFP:INCLUDE7TARGET.FCR' ) 
SINCLUDE ( ' VINCLUDE/UUTIMER.COM' ) 

INTEGER  TN 

TIMER (TN)  *  READ  TIMER!) 

END 

SUBROUTINE  STOP_TIMER (  TN  ) 
SINCLUDE ( ' : PFP: INCLUDE/TARGET. FOR ' ) 
SINCLUDE ( 1 VINCLUDE/UUTIMER.COM' ) 

INTEGER  TN 


TIMER (TN)  =  TIMER (TN)  -  READ_TIMER ( ) 

NUMBERJTIMER  (4,  TN)  =  NUMBERJTIMER  (4,  TN)  t  1 
NUMBERJTICKS (4, TN)  =  NUMBERJTICKS ( 4 , TN)  *  DBLE (TIMER (TN) ) 


IF  (  NUMBERJTIMER (4,  TN) 
NUMBER_TIMER ( 1 , TN)  - 
NUMBERJTICKS <1,  TN)  = 
ELSEIF  (  NUMBER  TIMER (1 
NUMBERJTIMER  (2,  TN)  - 
NUMBERJTICKS (2,  TN)  = 
ELSE 

NUMBER_TIMER (3, TN)  - 
NUMBERJTICKS  (3,  TN)  = 
ENDIF 
END 


■LT.  STAGE1  )  THEN 
NUMBERJTIMER (1, TN) 
NUMBER_TICKS ( 1 , TN) 
•LT.  STAGE2  ) 
NUMBERJTIMER  (2,  TN) 
NUMBERJTICKS (2, TN) 

NUMBERJTIMER (3, TN) 
NUMBER  TICKS (3, TN) 


+  1 

+  DBLE (TIMER (TN) ) 
THEN 
+  1 

+  DBLE (TIMER (TN) ) 
*■  1 

+  DBLE (TIMER (TN) ) 


SUBROUTINE  OUTPUTJTIMER ( ) 

SINCLUDE ( ' : PFP: INCLUDE/ TARGET. FOR' ) 

SINCLUDE  (  '  VINCLUDE/UUTIMER.COM'  ) 

INTEGER  BN,  TN 
INTEGER* 4  AVERAGE 

DO  20  TN=1, 500 

IF  (  NUMBERJTIMER (4, TN)  . NE .  0  )  THEN 

CALL  OUTPUT_MESSAGE ( %VAL ( SIGNED1 6BIT) , TN, %VAL(INT2 (1 ) ) ) 
CALL  OUTPUT_MESSAGE  ( WAL  (CHARACTER_08BIT!  ,  'TIMER  '  ) 


DO  10  BN«1, 4 


noon  non  noon  non 
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IF  (  NUMBER_TIMER (BN, IN)  .ME.  0  )  THEN 
AVERAGE  =  INT4 (NUMBER_TICKS (BN. TN) / 

S  DBLE <NUMBER_TIMER (BN,  TN)  )  ) 

ELSE 

AVERAGE  =  0 
ENDIF 

CALL  OUTPUT_MESSAG£  (%VAL  (SIGNED_323IT)  .AVERAGE, 

S  %VAL ( INT2  ( 1 ) ) ) 

10  CONTINUE 

CALL  OUTPUT_NL 
END  IF 
20  CONTINUE 
END 


FILE :  uuv22 .19g/sutility/uuelu2ei.for 


SUBROUTINE  TLU2EI  (  X, 

Y,  F,  I,  J,  TBVAL  ! 

SUBROUTINE  NAME  : 

AERO 

AU*fiCf<  : 

B.  HI  l.** 

FUNCTION  : 

PERFORMS  A  LINEAR  TABLE  LOOK-UP 
WITH  TWO  INDEPENDENT  VARIABLES, 
INDICES  POINTING  TO  THE  AREA  OF 
IN  USE 

OF  A  TABLE 
AND  RETURNS 
THE  TABLE 

CALLED  FROM  : 

AERO,  BAUTO 

SUBROUTINES  CALLED  : 

ABS 

INPUTS  : 

X,  Y,  F 

OUTPUTS  : 

I, J, TBVAL 

UPDATES  : 

D.  SMITH  -  CR  *  59 

IMPLICIT  REAL  (A-H) 

IMPLICIT  REAL  <0-Z) 

REAL  Ft  3  ) 

EQUIVALENCE  (N12,  NYU),(N2I,  NXL) , (N22,  NXU),(N11,  ISP) 
EQUIVALENCE  (  DX,  XX), (  DY,  YY) 


COMPUTE  UPPER  AND  LOWER  BOUNDS  ON  INDICES  FOR  XX  AND  YY 


NXU  =  ABSI  F ( 1 )  )  +  .1 

MP1  =  ABS (  F  (2)  )  +  1.1 

NYU  =  MP1  +  1 

NXL  =  NYU  +  1 

NXU  =  NXU'MPl  +  2 

JS  =  J 

IS  =  I 

XX  =  X 

YY  =  Y 

IF ( <  F  1 1 )  . GE .  0.0  )  . AND .  (  F  < 2 )  .GE.  0.0  ))  GO  TO  5 

SWAP  THE  INDEPENDENTS  -  MIRROR  IMAGE  TABLE  WITH  FIXED 
PROGRAM  CALLING  SEQUENCE 


XX  =•  Y 
YY  =  X 
5  CONTINUE 


GET  POINTERS  WITHIN  LIMITS 


IF (  IS  .LT.  NXL  )  IS  =  NXL 
IF(  JS  .LT.  3  )  JS  =  3 


FIND  GREATEST  LOWER  BOUND  ON  INDEX  FOR  YY 
(UNLESS  YY  IS  OFF  THE  TABLE) 

10  CONTINUE 

JSP  -  .  9  +■  1 
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IF (  YY  -LE.  F(JSP)  )  GO  TO  30 
IF (  JS?  .EQ.  NYU  )  GO  TO  100 
JS  =  JSP 
GO  TO  10 

CONTINUE 

IF (  JS  .EQ.  3  )  GO  TO  100 
JS  *  JS  -  1 

CONTINUE 

IF (  YY  .LT.  F (JS)  '  GO  TO  20 

FIND  GREATEST  LOWER  BOUND  ON  INDEX  FOR  XX 
(UNLESS  XX  IS  OFF  THE  TABLE) 

CONTINUE 

ISP  =  IS  +  MP 1 

IF (  XX  .LE.  F (ISP)  )  GO  TO  300 
IF  (  ISP  .EQ.  NXU  )  GO  TO  400 
IS  »  ISP 
GO  TO  100 

CONTINUE 

IF (  IS  EQ.  NXL  )  GO  TO  400 
IS  =  IS  -  MP1 

CONTINUE 

IF (  XX  .LT.  F (IS)  )  GO  TO  200 
CONTINUE 


SET  UP  INDEXING* 
(INTERPOLATING) 


YY  F ( JS+1 ) 


F ( IS)  *  F (Nil) 

* 

XX  *  XJ 

F ( IS+MP1 )  *  F ( N21 ) 


F  (N1 2  )  * 


DOUBLE  XJP1 


F  (N22 ) 


Nil  *  IS  +  JS  - 

N12  =  Nil  +  1 

N21  -  Nil  +  MP1 

N22  -  N21  +  1 


IS  +  MP1 

(  XX  -  F  (IS)  )/(  F(IPMPl)  -  F  (IS)  ) 

(  F (N21 )  -  F (Nil)  )*DX  +  F (Nil) 

(  F  (N22)  -  F(N12)  )  *DX  *  F(N12) 

(  YY  -  FIJS)  )/(  F(JS  +  1)  -  F  (JS)  ) 


J  =  JS 

I  =  IS 

TBVAL  =  (  XJP1  -  XJ  ) *DY  *  XJ 


RETURN 

END 


FILE:  uuv22 . 1 9g/ sut i 1 i ty /uu vest hi .for 


SUBROUTINE  VCSTH1 (T, CG, TBURNM, IVCS, TOFFLT, TIMONV, DTOFFV, TVTAB, 
.  FOFF1 ,  FOFF2,  IV TAB,  r  aVC'  ,  rwcS,  FZVCS,  MXVCS,  MYVCS,  MZVCS,  MDOTV) 


SUBROUTINE  NAME 
AUTHOR (S)  : 


FUNCTION 


RESOLVES  THE  VCS  THRUSTER  BURN  TIMES 
THEIR  AePROPmATE  FORCES  AND  MOMENTS 


CALLED  FROM  : 


FORTRAN  MAIN 


o  n  o 


19.  Appendix  N:  EXOSIM  2.0  (End-to-end) 


SUBROUTINES  CALLED 


OUTPUTS  : 


T,  CG, TBURNM, IVCS, TCFFLT, TIMONV, DTOFFV, 
TVTAB, FOFF1 , FOFF2 

FXVCS, FYVCS, FZVCS, MXVCS, MYVCS, MZVCS, MDOTV 


c 

UPDATES  : 

D. 

SISSOM 

- 

CR 

# 

017 

c 

B. 

HILL 

- 

CR 

* 

030 

c 

D. 

SISSOM 

- 

CR 

# 

032 

c 

B. 

HILL 

- 

CR 

# 

038 

c 

T. 

THORNTON 

- 

CR 

# 

043 

c 

B. 

HILL 

~ 

CR 

* 

051 

c 

B. 

HILL 

- 

CR 

« 

057 

c 

n 

SMITH 

- 

CR 

# 

059 

c 

D. 

SISSOM 

“ 

CR 

* 

069 

c 

D. 

SMITH 

- 

CR 

* 

074 

c 

D. 

SMITH 

- 

CR 

* 

076 

c 

D. 

SMITH 

- 

CR 

# 

080 

c 

B. 

HILL  / 

- 

CR 

# 

081 

c 

R. 

RHYNE 

c 

D. 

SMITH 

- 

CR 

* 

06z 

c 

R. 

RHYNE 

- 

CR 

* 

084 

c 

B. 

KILL 

- 

CR 

* 

086 

c 

R. 

RHYNE 

- 

CR 

# 

087 

c 

B. 

HILL 

- 

CR 

* 

089 

c 

B. 

HILL 

- 

CR 

* 

093 

IMPLICIT  REAL 
IMPLICIT  REAL 

REAL  ATHRV ( 4 ) 
REAL  F ( 3 ) 

REAL  FOFF1I4) 
REAL  M  (3) 

REAL  MYVCS 
REAL  TMVCS  (6,4) 
REAL  VCSDIR(3,4) 
REAL  VOFFi ( 4 ) 

INTEGER 

INTEGER 


CG  (3) 

FO  (3) 

FOFF2 (4) 
MDOTV 
MZVCS 
TOFFLT (4) 
VCSLOC (3,4) 
VOFF2 ( 4 ) 


DTOFFV ( 4 ) 

ISPVCS 
MXVCS 
THVCS (6, 4) 

VCSMA  (9,  4) 
XMOT (3) 


INDX (4) 
LENVCS (4) 


C  LOCAL  DATA  USED  FOR  CONSTANTS,  VARIABLES  AND  INITIALIZATION  FLAG 

SAVE  IVCSTH  ,  VCSMA 

*  DATA  INITIALIZATION 

SINCLUDE ( 1  * / INCLUDE /SSVCSTHR . DAT ' ) 

SINCLUDE ( ' * / INCLUDE /SSCON70 . DAT ' ) 

5INCLUDE  ( '  "VINCLUDE/SSCON09.  DAT  • ) 

DATA  IVCSTH  /  1  / 

IF  (IVCSTH. EQ.l)  THEN 


VCS  MISALIGNMENT  DIRECTIONS 
VOFFI  -  CONE  ANGLE  OFF  NORMAL 
VOFF2  «  POLAR  ANGLE 

DO  10  I  =1,4 

VOFFI (I)  =  FOFF1 (I) 

VOFF2 (I)  =  FOFF2 (I) 
CONTINUE 


VCS  THRUSTER  MISALIGNMENT  MATRIX 

DO  200  1=1,4 

CVOFF1  =  COS (VOFFI (I) ) 
svoffi  =  mrrvoFt  ±  m  > 

CVOFF2  =  COS (VOFF2 (I) ) 

SVOFF2  =  SIN (VOFF2 (I) ) 

VCSMA (1, I)  =  CVOFF1 
VCSMA (2, I)  =  SVOFFI *CVOFF2 
VCSMA (3,1)  =  SVOFFI *SVOFF2 


non 
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VCSMA <4, I)  =  SV0FF1 'SV0FF2 
VCSMA ( 5 , I)  =  CV0FF1 
VCSMA <6, I)  =  SV0FF1*CV0FF2 
VCSMA (7, I)  =  SV0FF1 *CV0FF2 
VCSMA (8,  I)  =  SV0FF1*SV0FF2 
VCSMA (9, I)  =  CV0FF1 
200  CONTINUE 
ENDIF 

C  RESET  THE  FORCE  AND  MOMENT  TO  ZERO 

FXVCS  =0.0 
FYVCS  =0.0 
FZVCS  =0.0 
MXVCS  =0.0 
MYVCS  =  0.0 
MZVCS  =  0.0 
MDOTV  =  0.0 

IF  (IVTA8  .EQ.  1)  THEN 

*  The  IVTAB  assignment  was  moved  to  the  partition  with  VCSLOG 

*  IVTAB  =  0 

IF  (  TBURNM  .GE.  TCMINV  )  THEN 

C  DEFINE  VCS  THRUST  PROFILE 

TMVCS (1, IVCS)  =  TVTAB 

THVCS (1 , IVCS )  =0.0 

TMVCS (2, IVCS)  =  TIMONV 

THVCS (2, IVCS)  =0.0 

TMVCS (3, IVCS)  =  TIMONV  4  TRUPV 

THVCS (3, IVCS)  =  FLATM 

TMVCS (4, IVCS)  =  TIMONV  4  TBURNM 

THVCS (4, IVCS)  =  FLATM 

TMVCS (5, IVCS)  =  TMVCS (4, IVCS)  4  TRDNV 

THVCS (5, IVCS)  =0.0 

TMVCS (6, IVCS)  =  999.0 

THVCS (6, IVCS)  =0.0 

LENVCS ( IVCS)  =  6 

TBURNM  =0.0 

ENDIF 

C  GENERATE  THRUSTER  RESPONSE  CURVE 

DO  15  1=1,4 

IF  (  DTOFFV(I) ,GT.0.0  )  THEN 
TMVCS (1,1)  =  TVTAB 
THVCS (1,1)  =0.0 
TMVCS  (2,  I)  =  TVTAB  4-  TLAGV 
THVCS (2, I)  =0.0 
TMVCS (3, I)  =  TMVCS (2, I)  4  TRUPV 
THVCS (3, I)  =  FLATM 
TMVCS (4, I)  =  TOFFLT ( I ) 

THVCS (4, I)  =  FLATM 
TMVCS (5, I)  =  TMVCS (4, I)  4  TRDNV 
THVCS (5,1)  =  0.0 
TMVCS  (6,  1)  =  999.0 
THVCS (6, I)  =  0.0 
LENVCS ( I )  =6 

ENDIF 

15  CONTINUE 

ENDIF 

C  SET  TABLE  LOOKUP  REFERENCE  TIME 

TRF.F  =  T 
DO  20  1=1,4 

COMPUTE  INSTANTANEOUS  THRUST  LEVEL  VIA  TABLE  LOOKUP  IF  VCS 
CYCLE  IS  SCHEDULED  FOR  THIS  THRUSTER  .  ALSO  EXTRAPOLATE  TIME 
OF  NEXT  V<~s  TABLE  LOOKUP  INDEX  TRANSITION  . 

IF  (  TMVC-,  U,  Ii  .GT.0.0  )  THEN 

CALL  TABLE (TMVCS (1, I) , THVCS (1, I) , TREF, ATHRV (I ) , 

LENVCS (I) , INDX (I) ) 


ELSE 


o  o  o  o  o 


19.  Appendix  N:  EXO.SIM  2.0  (End-to-end) 
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ATHRV(I)  =0.0 
INDX(I)  =  0 
ENDIF 

CALCULATE  FORCES  AND  MOMENTS  DUE  TO  THE  VCS  THRUSTERS  : 
F { I )  IS  THE  FORCE  ALONG  THE  Ith  AXIS. 

XMOT(I)  IS  THE  EFFECTIVE  MOMENT  ARM. 

FORCES  ARE  ADJUSTED  FOR  MISALIGNMENT  EFFECTS. 

THE  MOMENT  GENERATED  IS  {  F  x  XMOT) . 


DO  25  J=l, 3 

F0  < J )  =  VCSDIRtJ, I) 'ATHRV(I) 

XMOT  { J)  =  CG(J)  -  VCSLOC(J,I) 


CONTIS 

F  (1 ) 

TtC 

=  VCSMA  (1,  I)  *F0  (1 

)  +  VCSMA  14,1] 

i  *F0 (2) 

*  VCSMA  (7,  I) 

*F0 13) 

F  ( 2 ) 

=  VCSMA ( 2 , I) *F0 (1 

)  =VCSMA(5,I1 

i  *F0 (2) 

+ VCSMA (8, I) 

"F0 (3) 

F  (3) 

•  VCSMA (3, 11 *F0  (1 

)  + VCSMA (6,1) 

i  *r  0  (2) 

tVCSMA  t  9,  I) 

*F0 (3) 

M  (1 ) 

M  (2) 

M  (3) 

=  F (2) *XMOT (3)  - 
=  F (3) *XMOT  (1!  - 
=  F (1)  *XMGT  ( 2 ,  - 

F (3) *XMOT (2) 

F (1) *XMOT (3) 

F (2) *XMOT (1) 

FXVCS  =  FXVCS  +  F(l) 

FYVCS  =  FYVCS  +  F ( 2 ) 

FZVCS  =  FZVCS  +  F ( 3 ) 

MX VCS  »  MXVCS  +  M (1 ) 

MYVCS  =  MYVCS  +  M  <  2 ) 

MZVCS  =  MZVCS  +  M (3) 

MDOTV  =  MDOTV  +  ATHRV ( I ) / ISPVCS 
20  CONTINUE 


END 


